optimal & fix file cmake

CMakeLists.txt

@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(mission_adapters VERSION 1.0.0 LANGUAGES CXX)
+project(mission_adapters VERSION 0.2.0 LANGUAGES CXX)
 
 # ========================================================
-# Detect build mode
+# Build mode detection
 # ========================================================
 if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
   set(BUILDING_WITH_CATKIN TRUE)
@@ -12,6 +12,7 @@ else()
   message(STATUS "Building mission_adapters with Standalone CMake")
 endif()
 
+
 # ========================================================
 # C++ Standard
 # ========================================================
@@ -19,16 +20,22 @@ set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
-# ========================================================
-# Common dependencies
-# ========================================================
-find_package(PCL REQUIRED)
-find_package(OpenMP REQUIRED)
-find_package(Boost REQUIRED COMPONENTS system)
-find_package(Eigen REQUIRED)
 
 # ========================================================
-# Standalone mode
+# Common dependencies
+#
+# Chỉ những thứ code thật sự dùng: message contract (robot_protocol_msgs / robot_geometry_msgs),
+# tiện ích ROS-like (robot_cpp / robot_time) và Boost header (boost::shared_ptr trong message).
+# PCL / OpenMP / Eigen của bản trước là di sản copy từ template planner, gói này không dùng.
+# ========================================================
+# Boost.DLL cần system + filesystem để nạp plugin theo library_path.
+find_package(Boost REQUIRED COMPONENTS system filesystem)
+find_package(Threads REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+
+# ========================================================
+# Standalone configuration
 # ========================================================
 if(NOT BUILDING_WITH_CATKIN)
 
@@ -38,174 +45,154 @@ if (NOT BUILDING_WITH_CATKIN)
   set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
   set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
 
-  # ⚠️ NOTE: Đây KHÔNG phải package thật -> chỉ để placeholder
+  # Test/<pkg> -> src/AMR_T800/pnkx_nav_core/src
+  set(PNKX_NAV_CORE_SRC_DIR
+    "${CMAKE_CURRENT_SOURCE_DIR}/../../pnkx_nav_core/src"
+  )
+
+  # Test/<pkg> -> T800_ws/devel/lib
+  set(WORKSPACE_DEVEL_LIB_DIR
+    "${CMAKE_CURRENT_SOURCE_DIR}/../../../../devel/lib"
+  )
+
+  file(GLOB STANDALONE_PACKAGE_INCLUDE_DIRS
+    LIST_DIRECTORIES true
+
+    ${PNKX_NAV_CORE_SRC_DIR}/*/include
+    ${PNKX_NAV_CORE_SRC_DIR}/*/*/include
+    ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/include
+    ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/*/include
+  )
+
+  set(STANDALONE_INCLUDE_DIRS
+    ${STANDALONE_PACKAGE_INCLUDE_DIRS}
+
+    /usr/local/include
+  )
+
   set(PACKAGES_DIR
-    robot_costmap_2d
-    robot_nav_core
-    robot_nav_core2
-    robot_nav_msgs
-    robot_std_msgs
-    robot_geometry_msgs
     robot_cpp
-    robot_tf3_geometry_msgs
+    robot_time
-    robot_visualization_msgs
+    robot_xmlrpcpp
-    robot_nav_2d_utils
-    data_convert
   )
 
-  find_library(TF3_LIBRARY
+  if(EXISTS ${WORKSPACE_DEVEL_LIB_DIR})
-    NAMES tf3
+    link_directories(${WORKSPACE_DEVEL_LIB_DIR})
-    PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu
-  )
-
-  if(NOT TF3_LIBRARY)
-    message(FATAL_ERROR "❌ tf3 library not found")
   endif()
 
+  link_directories(/usr/local/lib)
+
 # ========================================================
-# Catkin mode
+# Catkin configuration
 # ========================================================
 else()
 
   find_package(catkin REQUIRED COMPONENTS
-    robot_costmap_2d
-    robot_nav_core
-    robot_nav_core2
-    robot_nav_msgs
-    robot_std_msgs
-    robot_geometry_msgs
     robot_cpp
-    robot_tf3_geometry_msgs
+    robot_time
-    robot_visualization_msgs
+    robot_geometry_msgs
-    robot_nav_2d_utils
+    robot_protocol_msgs
-    data_convert
+    robot_std_msgs
   )
 
-  find_library(TF3_LIBRARY
-    NAMES tf3
-    PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu
-  )
-
-  if(NOT TF3_LIBRARY)
-    message(FATAL_ERROR "❌ tf3 library not found")
-  endif()
-
   catkin_package(
-    INCLUDE_DIRS include
+    INCLUDE_DIRS
-    LIBRARIES ${PROJECT_NAME}
+      include
+
+    LIBRARIES
+      mission_adapters
+
     CATKIN_DEPENDS
-      robot_costmap_2d
-      robot_nav_core
-      robot_nav_core2
-      robot_nav_msgs
-      robot_std_msgs
-      robot_geometry_msgs
       robot_cpp
-      robot_tf3_geometry_msgs
+      robot_time
-      robot_visualization_msgs
+      robot_geometry_msgs
-      robot_nav_2d_utils
+      robot_protocol_msgs
-      data_convert
+      robot_std_msgs
-    DEPENDS PCL Boost
+
+    DEPENDS
+      Boost
   )
 
-  include_directories(
+  include_directories(include)
-    include
+
+  # SYSTEM: header của dependency (robot_cpp/console.h khai hàng chục hằng màu không dùng) sẽ ngập
+  # warning dưới -Wall -Wextra và che mất warning của chính gói này.
+  include_directories(SYSTEM
     ${catkin_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
     ${Boost_INCLUDE_DIRS}
   )
 
 endif()
 
-# ========================================================
-# Libraries
-# ========================================================
 
-# # utils lib
+# ========================================================
-# add_library(${PROJECT_NAME}_utils SHARED
+# Core library
-#   src/angle_utils.cpp
+# ========================================================
-#   src/config.cpp
+add_library(mission_adapters SHARED
-# )
+  src/types.cpp
-
+  src/event.cpp
-# main planner lib
+  src/mission_manager.cpp
-add_library(${PROJECT_NAME} SHARED
+  src/mission_executor.cpp
-  src/mission_adapters.cpp
+  src/event_processor.cpp
+  src/mission_config.cpp
+  src/plugin_registry.cpp
 )
 
+# Gói này KHÔNG kế thừa cờ -w của pnkx_nav_core: warning ở đây phải nhìn thấy được.
+target_compile_options(mission_adapters PRIVATE -Wall -Wextra)
+
+target_include_directories(mission_adapters
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+
 # ========================================================
-# Catkin linking
+# Linking
 # ========================================================
 if(BUILDING_WITH_CATKIN)
 
-  # add_dependencies(${PROJECT_NAME}_utils ${catkin_EXPORTED_TARGETS})
+  add_dependencies(mission_adapters
-  add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
+    ${${PROJECT_NAME}_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
+  )
 
-  # target_include_directories(${PROJECT_NAME}_utils
+  target_link_libraries(mission_adapters
-  #   PUBLIC
-  #     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  #     $<INSTALL_INTERFACE:include>
-  # )
-
-  target_include_directories(${PROJECT_NAME}
     PUBLIC
-      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      ${catkin_LIBRARIES}
-      $<INSTALL_INTERFACE:include>
+
+    PRIVATE
+      Boost::boost
+      Boost::system
+      Boost::filesystem
+      Threads::Threads
+      ${CMAKE_DL_LIBS}
+      yaml-cpp
   )
 
-  # target_link_libraries(${PROJECT_NAME}_utils
-  #   PUBLIC ${catkin_LIBRARIES}
-  #   PRIVATE Boost::system
-  #   ${TF3_LIBRARY}
-  # )
-
-  target_link_libraries(${PROJECT_NAME}
-    # PUBLIC ${PROJECT_NAME}_utils
-    PUBLIC ${catkin_LIBRARIES}
-    PUBLIC ${PCL_LIBRARIES}
-    PRIVATE Boost::system
-    ${TF3_LIBRARY}
-    OpenMP::OpenMP_CXX
-  )
-
-# ========================================================
-# Standalone linking
-# ========================================================
 else()
 
-  # target_include_directories(${PROJECT_NAME}_utils
+  target_include_directories(mission_adapters
-  #   PUBLIC
+    PRIVATE
-  #     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      ${STANDALONE_INCLUDE_DIRS}
-  #     $<INSTALL_INTERFACE:include>
+      ${Boost_INCLUDE_DIRS}
-  # )
+  )
 
-  target_include_directories(${PROJECT_NAME}
+  target_link_libraries(mission_adapters
     PUBLIC
-      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      ${PACKAGES_DIR}
-      $<INSTALL_INTERFACE:include>
+
+    PRIVATE
+      Boost::boost
+      Boost::system
+      Boost::filesystem
+      Threads::Threads
+      ${CMAKE_DL_LIBS}
+      yaml-cpp
   )
 
-  # target_link_libraries(${PROJECT_NAME}_utils
+  set_target_properties(mission_adapters PROPERTIES
-  #   PUBLIC ${PACKAGES_DIR}   # ⚠️ placeholder
-  #   PRIVATE Boost::system
-  #   ${TF3_LIBRARY}
-  # )
-
-  target_link_libraries(${PROJECT_NAME}
-    # PUBLIC ${PROJECT_NAME}_utils
-    PUBLIC ${PACKAGES_DIR}   # ⚠️ placeholder
-    PUBLIC ${PCL_LIBRARIES}
-    PRIVATE Boost::system
-    ${TF3_LIBRARY}
-    OpenMP::OpenMP_CXX
-  )
-
-  # set_target_properties(${PROJECT_NAME}_utils PROPERTIES
-  #   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  #   BUILD_RPATH "${CMAKE_BINARY_DIR}"
-  #   INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
-  # )
-
-  set_target_properties(${PROJECT_NAME} PROPERTIES
     LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
     BUILD_RPATH "${CMAKE_BINARY_DIR}"
     INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
@@ -213,12 +200,110 @@ else()
 
 endif()
 
+
+# ========================================================
+# Plugins — mỗi nguồn mission là một .so riêng.
+#
+# Tên file .so phải khớp khoá `library_path` trong
+# `pnkx_nav_core/config/mission_adapters_params.yaml`; tên symbol export phải khớp khoá `type`
+# trong `mission_sources`. Lệch một trong hai thì build vẫn sạch còn runtime báo "không tìm thấy".
+# ========================================================
+set(MISSION_ADAPTERS_PLUGIN_TARGETS
+  mission_adapters_goal_source
+  mission_adapters_vda5050_source
+)
+
+add_library(mission_adapters_goal_source SHARED plugins/goal_source_adapter.cpp)
+add_library(mission_adapters_vda5050_source SHARED plugins/vda5050_source_adapter.cpp)
+
+foreach(plugin_target ${MISSION_ADAPTERS_PLUGIN_TARGETS})
+  target_compile_options(${plugin_target} PRIVATE -Wall -Wextra)
+
+  set_target_properties(${plugin_target} PROPERTIES POSITION_INDEPENDENT_CODE ON)
+
+  target_include_directories(${plugin_target}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/plugins>
+  )
+
+  target_link_libraries(${plugin_target}
+    PUBLIC
+      mission_adapters
+
+    PRIVATE
+      Boost::boost
+      Threads::Threads
+  )
+
+  if(NOT BUILDING_WITH_CATKIN)
+    target_include_directories(${plugin_target} PRIVATE ${STANDALONE_INCLUDE_DIRS})
+    target_link_libraries(${plugin_target} PRIVATE ${PACKAGES_DIR})
+
+    set_target_properties(${plugin_target} PROPERTIES
+      LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+      BUILD_RPATH "${CMAKE_BINARY_DIR}"
+      INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+    )
+  endif()
+endforeach()
+
+
+# ========================================================
+# Example
+#
+# Có build để tài liệu không trôi khỏi API: đổi contract mà quên ví dụ thì build gãy ngay.
+# ========================================================
+option(BUILD_MISSION_ADAPTERS_EXAMPLES "Build mission_adapters examples" ON)
+
+if(BUILD_MISSION_ADAPTERS_EXAMPLES)
+
+  add_executable(robot_control_example examples/robot_control_example.cpp)
+
+  target_compile_options(robot_control_example PRIVATE -Wall -Wextra)
+
+  target_link_libraries(robot_control_example
+    PRIVATE
+      mission_adapters
+      Threads::Threads
+  )
+
+  if(NOT BUILDING_WITH_CATKIN)
+    target_include_directories(robot_control_example
+      PRIVATE
+        ${STANDALONE_INCLUDE_DIRS}
+    )
+
+    target_link_libraries(robot_control_example PRIVATE ${PACKAGES_DIR})
+  endif()
+
+  # Demo: chạy một VDA5050 Order qua VDA5050SourceAdapter và in ra các chặng thu được.
+  # Link thẳng plugin source adapter thay vì nạp qua boost::dll — demo chỉ cần xem kết quả cắt
+  # chặng, không cần kiểm tra đường nạp plugin.
+  add_executable(vda5050_order_demo examples/vda5050_order_demo.cpp)
+
+  target_compile_options(vda5050_order_demo PRIVATE -Wall -Wextra)
+
+  target_link_libraries(vda5050_order_demo
+    PRIVATE
+      mission_adapters
+      mission_adapters_vda5050_source
+      Threads::Threads
+  )
+
+  if(NOT BUILDING_WITH_CATKIN)
+    target_include_directories(vda5050_order_demo PRIVATE ${STANDALONE_INCLUDE_DIRS})
+    target_link_libraries(vda5050_order_demo PRIVATE ${PACKAGES_DIR})
+  endif()
+
+endif()
+
+
 # ========================================================
 # Install
 # ========================================================
 if(BUILDING_WITH_CATKIN)
 
-  install(TARGETS ${PROJECT_NAME}
+  install(TARGETS mission_adapters ${MISSION_ADAPTERS_PLUGIN_TARGETS}
     ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
     RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
@@ -229,15 +314,9 @@ if(BUILDING_WITH_CATKIN)
     FILES_MATCHING PATTERN "*.h"
   )
 
-  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml)
-    install(FILES plugins.xml
-      DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-    )
-  endif()
-
 else()
 
-  install(TARGETS ${PROJECT_NAME}
+  install(TARGETS mission_adapters ${MISSION_ADAPTERS_PLUGIN_TARGETS}
     EXPORT ${PROJECT_NAME}-targets
     ARCHIVE DESTINATION lib
     LIBRARY DESTINATION lib
@@ -255,56 +334,71 @@ else()
     FILES_MATCHING PATTERN "*.h"
   )
 
-  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml)
-    install(FILES plugins.xml
-      DESTINATION share/${PROJECT_NAME}
-    )
 endif()
 
-endif()
 
 # ========================================================
-# Unit Test
+# Tests
+#
+# Gói này đăng ký test với catkin/ctest (chạy được qua `catkin_make run_tests`) VÀ mở
+# EXCLUDE_FROM_ALL để binary có mặt sau `catkin_make` thường — chạy thẳng
+# ./devel/lib/mission_adapters/<test> theo đúng quy trình verify của repo.
 # ========================================================
-if(CATKIN_ENABLE_TESTING)
+if(CATKIN_ENABLE_TESTING AND BUILDING_WITH_CATKIN)
 
-  find_package(catkin REQUIRED COMPONENTS
+  if(NOT COMMAND catkin_add_gtest)
-    robot_costmap_2d
-    robot_nav_core
-    robot_nav_core2
-    robot_nav_msgs
-    robot_std_msgs
-    robot_geometry_msgs
-    robot_cpp
-    robot_tf3_geometry_msgs
-    robot_visualization_msgs
-    robot_nav_2d_utils
-    data_convert
-  )
-
-  message(STATUS "CATKIN_ENABLE_TESTING=${CATKIN_ENABLE_TESTING}")
-
-  if(COMMAND catkin_add_gtest)
-    message(STATUS "catkin_add_gtest exists")
-  else()
     message(FATAL_ERROR "catkin_add_gtest NOT FOUND")
   endif()
 
-  catkin_add_gtest(test_mission_adapters
+  set(MISSION_ADAPTERS_TESTS
-    test/mission_adapters_test.cpp
+    event_bus_test
+    adapter_test
+    mission_manager_test
+    mission_lifecycle_test
+    plugin_registry_test
   )
 
-  if(TARGET test_mission_adapters)
+  foreach(test_name ${MISSION_ADAPTERS_TESTS})
-    target_link_libraries(test_mission_adapters
+    catkin_add_gtest(${test_name} test/${test_name}.cpp)
+
+    if(TARGET ${test_name})
+      set_target_properties(${test_name} PROPERTIES EXCLUDE_FROM_ALL FALSE)
+
+      target_compile_options(${test_name} PRIVATE -Wall -Wextra)
+
+      target_include_directories(${test_name}
+        PRIVATE
+          ${CMAKE_CURRENT_SOURCE_DIR}/include
+          ${CMAKE_CURRENT_SOURCE_DIR}/plugins
+          ${CMAKE_CURRENT_SOURCE_DIR}/test
+      )
+
+      target_include_directories(${test_name} SYSTEM
+        PRIVATE
+          ${catkin_INCLUDE_DIRS}
+      )
+
+      # catkin_add_gtest dùng target_link_libraries dạng plain, nên phần bổ sung cũng phải plain.
+      #
+      # Test link thẳng plugin .so để kiểm logic chuyển đổi ở mức unit; đường nạp qua Boost.DLL
+      # được kiểm riêng trong plugin_registry_test.
+      target_link_libraries(${test_name}
         mission_adapters
+        ${MISSION_ADAPTERS_PLUGIN_TARGETS}
         ${catkin_LIBRARIES}
         pthread
       )
 
-    target_include_directories(test_mission_adapters PRIVATE
+      # plugin_registry_test nạp .so từ devel/lib theo library_path, nên cần cả hai plugin đã build.
-      ${CMAKE_CURRENT_SOURCE_DIR}/include
+      add_dependencies(${test_name} ${MISSION_ADAPTERS_PLUGIN_TARGETS})
-      ${catkin_INCLUDE_DIRS}
+
+      # Đường tới cây config và thư mục .so của test, để binary tự trỏ đúng chỗ khi chạy qua ctest
+      # (ctest không mang theo PNKX_NAV_CORE_CONFIG_DIR hay LD_LIBRARY_PATH của shell).
+      target_compile_definitions(${test_name} PRIVATE
+        MISSION_ADAPTERS_TEST_CONFIG_DIR="${CMAKE_CURRENT_SOURCE_DIR}/test/config"
+        MISSION_ADAPTERS_TEST_LIBRARY_DIR="${CATKIN_DEVEL_PREFIX}/lib"
       )
     endif()
+  endforeach()
 
 endif()

README.md

@@ -1,461 +1,195 @@
-# Mission Adapters
+# mission_adapters
 
-Mission Adapters là một framework quản lý nhiệm vụ (Mission Management Framework) được xây dựng bằng C++ thuần, hướng tới các hệ thống AGV/AMR và tương thích với mô hình nhiệm vụ của VDA5050.
+Lớp mission độc lập ROS: nhận yêu cầu từ nguồn ngoài (goal đơn lẻ, VDA5050 Order), chuyển thành hàng
+đợi mission, và giữ trạng thái của hàng đợi đó.
 
-Framework cung cấp:
+Gói **không biết** navigation runtime nào đang chạy phía sau. Mission đi xuống qua cổng
+`NavigationClient`, kết quả quay về theo `MissionId`. Nhờ vậy cùng một lớp mission dùng lại được cho
+runtime khác, và test được mà không cần robot.
 
-* Chuyển đổi Goal hoặc VDA5050 Order thành Mission.
+Nguồn mission là plugin nạp bằng Boost.DLL theo khoá `library_path` trong YAML — xem
-* Quản lý hàng đợi Mission.
+[docs/PLUGIN_GUIDE.md](docs/PLUGIN_GUIDE.md).
-* Xử lý Event bất đồng bộ.
-* Điều phối Mission theo trạng thái Robot.
-* Hỗ trợ Pause / Resume / Cancel / Emergency.
-* Tách biệt Mission Scheduling và Mission Execution.
-* Không phụ thuộc ROS.
 
----
+## Luồng runtime
-
-# Kiến trúc tổng thể
 
 ```text
-Order / Goal
+   host (MQTT / OPC-UA / REST / UI)
-      │
+        │  goalEvent / orderEvent / submitRequest(MissionRequest)
         ▼
-+------------------+
+  ┌──────────────────┐   tra schema    ┌────────────────┐
-|  EventProcessor  |
+  │  EventProcessor  │ ──────────────► │ PluginRegistry │  (các .so nguồn mission)
-+------------------+
+  │  (thread event)  │ ◄────────────── └────────────────┘
-      │
+  └──────────────────┘  ConversionResult
+        │  submit(missions) / append(missions)
         ▼
-+------------------+
+  ┌──────────────────┐
-|  MissionManager  |
+  │  MissionManager  │   hàng đợi + MissionState, cấp MissionId
-+------------------+
+  └──────────────────┘
-      │
+        │  nextMission() / takePendingCancel()
         ▼
-+------------------+
+  ┌──────────────────┐
-| MissionExecutor  |
+  │ MissionExecutor  │   thread DUY NHẤT phát lệnh ra ngoài
-+------------------+
+  │  (thread exec)   │
-      │
+  └──────────────────┘
+        │  NavigationClient::dispatch(mission) / cancelActive(id)
         ▼
- Navigation Stack
+   navigation runtime
- (MoveBase, MPPI,
+        │  navDoneEvent(id) / navFailedEvent(id)
-  Pure Pursuit...)
+        └──────────────────────────► quay lại EventProcessor
 ```
 
----
+## Bất biến quan trọng
 
-# Thành phần chính
+Đây là những tính chất mà bộ test khoá lại; đọc phần này trước khi sửa code.
 
-## Mission
+- **Mọi outcome mang `MissionId`.** `navDoneEvent(id)` / `navFailedEvent(id)` chỉ được chấp nhận khi
+  `id` khớp mission đang chạy. Outcome đến trễ của một mission đã bị thay thế bị loại bỏ kèm log —
+  nếu không, kết quả của chặng cũ sẽ "hoàn thành" chặng mới mà robot chưa hề chạy.
+- **`submit()` với danh sách rỗng là no-op tuyệt đối.** Một order lỗi không phải lệnh huỷ việc đang
+  chạy; nó không được đụng tới hàng đợi.
+- **Mọi đường thoát của một mission đều bảo navigation dừng.** Bị thay thế (preempt), bị huỷ,
+  emergency, quá hạn — tất cả đều đi qua `NavigationClient::cancelActive()`. Mission layer quên
+  mission mà không bảo navigation dừng thì robot vẫn chạy tiếp tới goal cũ.
+- **Thứ tự sự kiện = thứ tự phát sinh (FIFO).** Phát order rồi huỷ ngay thì huỷ phải được xử lý sau
+  order, nếu không robot chạy đúng cái người dùng vừa huỷ. Ngoại lệ duy nhất là emergency: nó bật cờ
+  atomic ngay tại chỗ gọi nên độ trễ phản ứng không phụ thuộc độ dài hàng đợi.
+- **`nextMission()` trả mỗi mission đúng một lần**, tại đúng bước chuyển `QUEUED → RUNNING`. Bên gọi
+  không phải tự khử trùng lặp.
+- **Mission bất biến sau khi submit.** `MissionManager` gán `id` rồi chia sẻ dưới dạng
+  `shared_ptr<const Mission>` — ba thread đọc chung mà không cần khoá.
+- **`MissionManager` không bao giờ gọi ra ngoài khi đang giữ khoá.** Nó chỉ ghi "mission này cần
+  được dừng" vào ô pending-cancel; `MissionExecutor` mới là thread thực hiện lời gọi đó.
 
-Đơn vị thực thi cơ bản.
+## State machine
+
+```text
+IDLE ──submit──► QUEUED ──dequeue──► RUNNING ──nav_done──► còn mission? ──có──► QUEUED
+                                        │      (nav + action đều xong)  │
+                                        │ nav_failed / mission_timeout  │ không
+                                        ▼                               ▼
+                                     FAILED ──submit mới──► QUEUED   COMPLETED ──submit mới──► QUEUED
+
+PAUSED ◄── pause / resume ──► (state trước đó)
+cancel (mọi state active) ──► CANCELLED ──submit mới──► QUEUED
+EMERGENCY: từ MỌI state; clear_emergency ──► CLEAR_EMERGENCY ──submit mới──► QUEUED
+```
+
+`nav_done(id)` nghĩa là **cả chặng** hoàn tất — navigation lẫn action. Mission layer không có state
+chờ action riêng: action do navigation runtime thực thi, và mission layer giữ nguyên `RUNNING` trọn
+chặng.
+
+Mọi lần đổi trạng thái được log một dòng `from -> to (lý do, mission id)` ở mức info, và chỉ khi
+state đổi thật.
+
+## Kiểu dữ liệu
 
 ```cpp
+using MissionId = std::uint64_t;   // 0 = kInvalidMissionId
+
 class Mission
 {
 public:
-    int sequenceId;
+    MissionId id;          // MissionManager cấp khi submit, đơn điệu tăng
-
+    MissionType type;      // SIMPLE_GOAL | VDA5050_ORDER
-    MissionType type;
+    bool has_goal;         // false = mission chỉ-có-action, navigation bỏ qua phần di chuyển
-
+    robot_geometry_msgs::PoseStamped start;   // chỉ hợp lệ khi has_goal
-    int priority;
+    robot_geometry_msgs::PoseStamped goal;    // chỉ hợp lệ khi has_goal
-
+    std::vector<robot_protocol_msgs::Node> nodes;
-    PoseStamped start;
+    std::vector<robot_protocol_msgs::Edge> edges;
-
+    std::vector<Action> actions;               // đã sắp theo sequenceId, đi qua nguyên vẹn
-    PoseStamped goal;
-
-    std::vector<Node> nodes;
-
-    std::vector<Edge> edges;
-
-    std::vector<Action> actions;
 };
 ```
 
-Mission có thể được tạo từ:
+Mission là **self-contained**: consumer không phải suy goal ra từ `nodes.back()`. Adapter VDA5050
+set sẵn `start`/`goal` từ `nodePosition` (theta `[rad]` → quaternion quanh trục z).
 
-* Goal đơn giản
+`has_goal == false` bắt buộc đi kèm ít nhất một action; core từ chối cả lô nếu adapter vi phạm.
-* VDA5050 Order
 
----
+## VDA5050 conformance
 
-## Action
+`VDA5050SourceAdapter` chịu trách nhiệm ba điểm:
 
-Đại diện cho một hành động tại Node hoặc Edge.
+| Điểm | Hành vi |
+|---|---|
+| `released` (base/horizon) | Chỉ phần base được thực thi. Horizon là dự định của fleet manager, chưa được phép chạy. Order không điền `released` ở đâu cả thì cả order được coi là base, kèm log cảnh báo. |
+| `orderId` / `orderUpdateId` | `orderId` mới → thay hàng đợi (`kReplace`). Cùng `orderId` + `orderUpdateId` lớn hơn → chỉ sinh phần vừa release thêm và **nối tiếp** (`kAppend`). `orderUpdateId` không mới hơn → từ chối. |
+| `goal` / `start` | Set từ node cuối / node đầu của chặng, trong frame `global_frame` (mặc định `map`). VDA5050 `mapId` là danh tính bản đồ, không phải frame TF, nên không dùng làm `frame_id`. |
+
+Chặng được cắt tại mỗi node có action: robot chạy tới node đó rồi mới thực hiện action. Action ngay
+tại node xuất phát sinh ra một chặng `has_goal == false`.
+
+## Config
+
+Bản runtime: `pnkx_nav_core/config/mission_adapters_params.yaml`.
+Bản test: `test/config/mission_adapters_params.yaml`, chỉ được đọc khi chạy kèm
+`PNKX_NAV_CORE_CONFIG_DIR`.
+
+| Khoá | Đơn vị / mặc định | Ý nghĩa |
+|---|---|---|
+| `mission_adapters/mission_sources` | — | Danh sách nguồn mission `{name, type}` |
+| `mission_adapters/mission_timeout` | `[s]`, `0.0` | Trần thời gian cho một chặng; `0` = tắt. Quá hạn → chặng thất bại **và** navigation được bảo dừng |
+| `mission_adapters/clear_queue_on_failure` | `true` | Một chặng hỏng thì xoá sạch hàng đợi. Đặt `false` chỉ khi các mission độc lập với nhau |
+| `<type>/library_path` | — | Tên `.so` của plugin — **thiếu khoá này là lỗi runtime phổ biến nhất** |
+
+## Dùng
 
 ```cpp
-class Action
+mission_adapters::PluginRegistry registry;
-{
+robot::NodeHandle nh;
-public:
+registry.loadFromConfig(nh);                     // nạp nguồn mission từ YAML
-    int sequenceId;
 
-    ActionType type;
+mission_adapters::MissionConfig config;
+config.loadFromParams(nh);
 
-    robot_protocol_msgs::Action action;
+mission_adapters::MissionManager  manager(config);
-};
+mission_adapters::EventProcessor  processor(manager, registry);
+mission_adapters::MissionExecutor executor(manager);
+
+executor.setNavigationClient(&my_navigation_client);   // non-owning, phải sống lâu hơn executor
+processor.start();
+executor.start();
+
+processor.goalEvent(goal);                       // hoặc orderEvent(order) / submitRequest(request)
+// ... navigation runtime gọi processor.navDoneEvent(mission->id) khi chặng xong ...
 ```
 
-Các Action sẽ được sắp xếp theo `sequenceId`.
+Ví dụ đầy đủ: [`examples/robot_control_example.cpp`](examples/robot_control_example.cpp) — có build,
+nên nó không thể trôi khỏi API.
 
----
+## Build và test
 
-## GoalAdapter
+```bash
+catkin_make --pkg mission_adapters
+source devel/setup.bash
 
-Chuyển đổi Goal thành Mission.
+./devel/lib/mission_adapters/event_bus_test
+./devel/lib/mission_adapters/adapter_test
+./devel/lib/mission_adapters/mission_manager_test
+./devel/lib/mission_adapters/mission_lifecycle_test
 
-```cpp
+PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/mission_adapters/test/config \
-GoalAdapter adapter;
+  ./devel/lib/mission_adapters/plugin_registry_test
-
-auto missions =
-    adapter.convert(goal);
 ```
 
-Kết quả:
+Test cũng đăng ký với ctest: `cd build && ctest -R mission_adapters --output-on-failure`.
 
-```text
+## Thread
-Goal
- └── Mission
-```
 
----
+| Thread | Sở hữu | Đánh thức bởi |
+|---|---|---|
+| `EventProcessor` | hàng đợi sự kiện, gọi adapter | `EventBus::pop()` chờ sự kiện |
+| `MissionExecutor` | mọi lời gọi xuống `NavigationClient` | `MissionManager::waitForWork()` chờ condition variable |
+| host | phát sự kiện, nhận outcome | — |
 
-## VDA5050Adapter
+Chuyển đổi payload → mission chạy trên thread của `EventProcessor`, không phải thread host: adapter
+được phép có state, và chuyển đổi trên nhiều thread host sẽ tranh chấp state đó.
 
-Chuyển đổi Order thành danh sách Mission.
+`MissionExecutor` không poll. Trong một vòng nó luôn **cancel trước, dispatch sau** — chặng cũ phải
+được bảo dừng trước khi chặng mới bắt đầu.
 
-Ví dụ:
+## Chưa có
 
-```text
+- Mission persistence (mất điện giữa order thì hàng đợi mất).
-N1 ---- N2 ---- N3(Action)
+- VDA5050 instant actions.
-                 |
+- Mission priority queue — hàng đợi hiện là FIFO thuần.
-                 |
+- Multi-robot fleet.
-                 V
-               Mission A
-
-N3 ---- N4 ---- N5(Action)
-                 |
-                 |
-                 V
-               Mission B
-```
-
-Order sẽ được chia thành nhiều Mission tại các Node chứa Action.
-
----
-
-## EventBus
-
-Hàng đợi ưu tiên cho các Event.
-
-```cpp
-std::priority_queue<
-    Event,
-    std::vector<Event>,
-    EventCompare>;
-```
-
-Priority nhỏ hơn sẽ được xử lý trước.
-
-```cpp
-EMERGENCY = 0
-CANCEL    = 1
-RESUME    = 2
-PAUSE     = 3
-NAV_DONE  = 4
-ORDER     = 5
-```
-
----
-
-## EventProcessor
-
-Tiếp nhận Event từ bên ngoài.
-
-Ví dụ:
-
-```cpp
-event_processor.orderEvent(order);
-
-event_processor.pauseEvent();
-
-event_processor.resumeEvent();
-
-event_processor.cancelEvent();
-
-event_processor.emergencyEvent();
-```
-
-EventProcessor hoạt động trên một worker thread riêng.
-
----
-
-## MissionManager
-
-Quản lý trạng thái và hàng đợi Mission.
-
-Các trạng thái hỗ trợ:
-
-```cpp
-IDLE
-QUEUED
-RUNNING
-PAUSED
-WAITING_ACTION
-RECOVERY
-COMPLETED
-FAILED
-CANCELLED
-EMERGENCY
-```
-
-Chức năng:
-
-* Submit Mission
-* Lấy Mission tiếp theo
-* Pause
-* Resume
-* Cancel
-* Emergency
-* Navigation Done
-* Navigation Failed
-
----
-
-## MissionExecutor
-
-Thread chuyên lấy Mission từ MissionManager.
-
-Khi có Mission mới, callback sẽ được gọi.
-
-```cpp
-using MissionCallback =
-    std::function<
-        void(
-            const std::shared_ptr<Mission>&
-        )
-    >;
-```
-
-Đăng ký callback:
-
-```cpp
-mission_executor.setMissionCallback(
-    [&](const std::shared_ptr<Mission>& mission)
-    {
-        executeMission(*mission);
-    });
-```
-
----
-
-# Luồng hoạt động
-
-## Goal
-
-```text
-Goal
-  │
-  ▼
-GoalAdapter
-  │
-  ▼
-Mission
-  │
-  ▼
-MissionManager
-  │
-  ▼
-MissionExecutor
-  │
-  ▼
-Navigation
-```
-
----
-
-## Order
-
-```text
-Order
-  │
-  ▼
-VDA5050Adapter
-  │
-  ▼
-Mission A
-Mission B
-Mission C
-  │
-  ▼
-Mission Queue
-  │
-  ▼
-MissionExecutor
-```
-
----
-
-# Navigation Feedback
-
-Khi Navigation hoàn thành:
-
-```cpp
-event_processor.navDoneEvent();
-```
-
-Khi Navigation thất bại:
-
-```cpp
-event_processor.navFailedEvent();
-```
-
-Ví dụ:
-
-```cpp
-auto state =
-    move_base_ptr_->getFeedback()
-        ->navigation_state;
-
-if(state != prev_state_)
-{
-    if(state ==
-        State::SUCCEEDED)
-    {
-        event_processor.navDoneEvent();
-    }
-
-    if(state ==
-        State::ABORTED)
-    {
-        event_processor.navFailedEvent();
-    }
-
-    prev_state_ = state;
-}
-```
-
----
-
-# Ví dụ sử dụng
-
-## Khởi tạo
-
-```cpp
-MissionManager mission_manager;
-
-EventProcessor event_processor(
-    mission_manager);
-
-MissionExecutor mission_executor(
-    mission_manager);
-```
-
----
-
-## Start
-
-```cpp
-event_processor.start();
-
-mission_executor.start();
-```
-
----
-
-## Đăng ký callback
-
-```cpp
-mission_executor.setMissionCallback(
-    [&](const std::shared_ptr<Mission>& mission)
-    {
-        executeMission(*mission);
-    });
-```
-
----
-
-## Nhận Order
-
-```cpp
-robot_protocol_msgs::Order order;
-
-event_processor.orderEvent(order);
-```
-
----
-
-## Pause
-
-```cpp
-event_processor.pauseEvent();
-```
-
----
-
-## Resume
-
-```cpp
-event_processor.resumeEvent();
-```
-
----
-
-## Cancel
-
-```cpp
-event_processor.cancelEvent();
-```
-
----
-
-## Emergency Stop
-
-```cpp
-event_processor.emergencyEvent();
-```
-
----
-
-# Thread Model
-
-Framework sử dụng 2 worker thread:
-
-```text
-Thread 1
- └─ EventProcessor
-
-Thread 2
- └─ MissionExecutor
-```
-
-Navigation Stack hoạt động độc lập.
-
-```text
-Main Thread
-     │
-     ├─ Navigation
-     │
-     ├─ EventProcessor
-     │
-     └─ MissionExecutor
-```
-
----
-
-# TODO
-
-Các chức năng dự kiến bổ sung:
-
-* Action Executor
-* WAITING_ACTION state
-* ACTION_DONE event
-* ACTION_FAILED event
-* Recovery Framework
-* Mission Priority Queue
-* Mission Persistence
-* VDA5050 Instant Actions
-* Multi-Robot Fleet Support
-
----
-
-# License
-
-Internal Project.

cfg/stanley_local_planner_params.yaml

@@ -1,11 +0,0 @@
-LocalPlannerAdapter:
-  library_path: liblocal_planner_adapter
-  yaw_goal_tolerance: 0.017  
-  xy_goal_tolerance: 0.03 
-  min_approach_linear_velocity: 0.06
-
-StanleyLocalPlanner:
-# base_local_planner: "hybrid_local_planner/HybridLocalPlanner"
-# HybridLocalPlanner:
-    library_path: libstanley_local_planner
-    

docs/PLUGIN_GUIDE.md

@@ -0,0 +1,176 @@
+# Viết một nguồn mission mới
+
+Nguồn mission là plugin duy nhất của gói này. Thêm một loại nguồn (REST, fleet manager riêng,
+teach-pendant, file kịch bản...) không cần sửa dòng nào trong `src/`.
+
+Việc **thực thi action** không thuộc đây: adapter chỉ chuyển dữ liệu, navigation runtime mới là nơi
+chạy action.
+
+## 1. Chọn tên schema
+
+Schema là chuỗi định danh loại payload, và là thứ core dùng để định tuyến. Đặt tên theo miền dữ
+liệu, không theo tên hãng hay giao thức truyền tải:
+
+```
+vda5050.order          # đã có
+geometry.pose_stamped  # đã có
+json.pick_and_place    # ví dụ nguồn mới
+```
+
+Hai adapter khai cùng một schema sẽ bị registry từ chối — định tuyến khi đó phụ thuộc thứ tự nạp,
+tức phụ thuộc vào thứ tự dòng trong file YAML.
+
+## 2. Hiện thực `MissionSourceAdapter`
+
+```cpp
+#include <boost/dll/alias.hpp>
+#include <mission_adapters/adapter.h>
+
+namespace mission_plugins
+{
+
+class PickAndPlaceAdapter : public mission_adapters::MissionSourceAdapter
+{
+public:
+    static mission_adapters::MissionSourceAdapter::Ptr create()
+    {
+        return std::make_shared<PickAndPlaceAdapter>();
+    }
+
+    bool configure(const std::string& name, robot::NodeHandle& nh) override
+    {
+        // Param riêng nằm trong namespace tên instance.
+        nh.getParam(name + "/global_frame", global_frame_, std::string("map"));
+        return !global_frame_.empty();
+    }
+
+    std::string schema() const override { return "json.pick_and_place"; }
+
+    bool validate(const mission_adapters::MissionRequest& request,
+                  std::string& reason) const override
+    {
+        if (request.raw_payload.empty())
+        {
+            reason = "payload rỗng";
+            return false;
+        }
+        return true;
+    }
+
+    mission_adapters::ConversionResult
+    convert(const mission_adapters::MissionRequest& request) override
+    {
+        mission_adapters::ConversionResult result;
+        // ... parse request.raw_payload, dựng mission ...
+        return result;
+    }
+
+private:
+    std::string global_frame_ = "map";
+};
+
+}  // namespace mission_plugins
+
+BOOST_DLL_ALIAS(mission_plugins::PickAndPlaceAdapter::create, PickAndPlaceAdapter)
+```
+
+Bốn quy tắc bắt buộc:
+
+- **State là member, không phải `static` local.** Một tiến trình có thể chạy nhiều instance; state
+  static sẽ nối chúng lại và order của robot này ảnh hưởng robot kia.
+- **`validate()` chặn dữ liệu hỏng trước khi nó tới navigation**: NaN/Inf, quaternion không chuẩn
+  hoá được, payload thiếu trường. Trả `false` kèm `reason` cụ thể — chuỗi đó đi thẳng vào log.
+- **`convert()` trả rỗng nghĩa là "không có việc", KHÔNG phải lỗi và KHÔNG phải lệnh huỷ.** Core sẽ
+  bỏ qua và giữ nguyên hàng đợi đang chạy. Đây là bất biến A1: một order lỗi từ fleet manager không
+  được âm thầm xoá order đang chạy.
+- **Mission `has_goal == false` phải có ít nhất một action.** Mission không goal và cũng không
+  action là chặng không có việc gì để làm, và nó sẽ không bao giờ báo kết quả về — mission layer kẹt
+  `RUNNING` vĩnh viễn. Core kiểm lại điều này và bỏ cả lô nếu phát hiện.
+
+### `SubmitMode` — thay hay nối tiếp
+
+`ConversionResult::mode` quyết định mission mới quan hệ thế nào với hàng đợi đang có:
+
+| mode | Ý nghĩa | Dùng khi |
+|---|---|---|
+| `kReplace` (mặc định) | Thay toàn bộ hàng đợi, chặng đang chạy bị preempt và được bảo dừng | Yêu cầu mới, độc lập với việc đang làm |
+| `kAppend` | Chỉ thêm vào cuối hàng đợi, không đụng chặng đang chạy | Phần nối tiếp của chính yêu cầu đang chạy |
+
+`kAppend` là hình dạng của VDA5050 order update. Trả `kReplace` cho một bản cập nhật nghĩa là mỗi
+lần fleet manager release thêm horizon, robot lại huỷ và chạy lại chặng đang đi.
+
+## 3. Build thành `.so` riêng
+
+```cmake
+add_library(mission_adapters_pick_and_place SHARED plugins/pick_and_place_adapter.cpp)
+
+target_compile_options(mission_adapters_pick_and_place PRIVATE -Wall -Wextra)
+set_target_properties(mission_adapters_pick_and_place PROPERTIES POSITION_INDEPENDENT_CODE ON)
+
+target_link_libraries(mission_adapters_pick_and_place
+  PUBLIC mission_adapters
+  PRIVATE Boost::boost Threads::Threads
+)
+
+install(TARGETS mission_adapters_pick_and_place
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+```
+
+Thêm dependency cho plugin thì phải cập nhật **đủ bốn chỗ**: `CMakeLists.txt`, `package.xml`,
+install rule, và file YAML.
+
+## 4. Khai báo trong YAML — **đừng quên `library_path`**
+
+Trong `pnkx_nav_core/config/mission_adapters_params.yaml`:
+
+```yaml
+mission_adapters:
+  mission_sources:
+    - {name: pick_src, type: PickAndPlaceAdapter}   # type = tên symbol trong BOOST_DLL_ALIAS
+
+pick_src:
+  global_frame: map        # param riêng của instance, đọc trong configure()
+
+PickAndPlaceAdapter:
+  library_path: libmission_adapters_pick_and_place   # BẮT BUỘC
+```
+
+Ba tên phải khớp nhau:
+
+| Nơi | Giá trị |
+|---|---|
+| `BOOST_DLL_ALIAS(..., PickAndPlaceAdapter)` | tên symbol |
+| `mission_sources[].type` | **cùng** tên symbol |
+| `<type>/library_path` | tên file `.so` (không cần đuôi) |
+
+**Thiếu `library_path` là lỗi phổ biến nhất**: plugin build sạch, `.so` nằm đúng chỗ, nhưng runtime
+báo "không tìm thấy". Registry sẽ nêu đích danh khoá bị thiếu trong log.
+
+Tên không có đuôi `.so` được resolve qua `PNKX_NAV_CORE_LIBRARY_PATH`, `devel/lib`, rồi
+`LD_LIBRARY_PATH` — nên **phải `source devel/setup.bash`** trước khi chạy, nếu không plugin sẽ không
+được tìm thấy dù file có thật.
+
+## 5. Kiểm chứng
+
+```bash
+catkin_make --pkg mission_adapters
+source devel/setup.bash
+
+PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/mission_adapters/test/config \
+  ./devel/lib/mission_adapters/plugin_registry_test
+```
+
+Nên có test cho: payload hợp lệ, payload thiếu trường, payload chứa NaN/Inf, payload không sinh
+mission nào, và (nếu nguồn có state) hai yêu cầu liên tiếp.
+
+## 6. Nguồn không cần `.so`
+
+Nguồn biên dịch thẳng vào host thì bỏ qua bước 3 và 4, đăng ký trực tiếp:
+
+```cpp
+registry.registerAdapter(std::make_shared<PickAndPlaceAdapter>());
+```
+
+Đường này dùng trong test và cho host muốn kiểm soát vòng đời adapter; phần còn lại của luồng
+(validate, convert, định tuyến theo schema) giống hệt.

examples/robot_control_example.cpp

@@ -0,0 +1,101 @@
+/*********************************************************************
+ *
+ * Ví dụ tối thiểu: nối mission layer vào một navigation runtime.
+ *
+ * Điểm chính của ví dụ là hai chiều dữ liệu, không phải thuật toán:
+ *
+ *   host  --goalEvent/orderEvent-->  EventProcessor  -->  MissionManager
+ *                                                              |
+ *                                                     MissionExecutor
+ *                                                              |
+ *                                            NavigationClient::dispatch/cancelActive
+ *                                                              |
+ *   host  <--navDoneEvent(id)/navFailedEvent(id)--------  navigation runtime
+ *
+ * Outcome quay về BẮT BUỘC mang MissionId. Bản trước của ví dụ này poll trường trạng thái của
+ * navigation rồi suy ra "chặng nào vừa xong" — cách đó gán nhầm kết quả ngay khi có một order thay
+ * thế order đang chạy.
+ *
+ *********************************************************************/
+#include <cstddef>
+#include <memory>
+
+#include <robot/robot.h>
+
+#include <mission_adapters/mission_adapters.h>
+#include <mission_adapters/plugin_registry.h>
+
+namespace
+{
+
+/**
+ * @brief NavigationClient chỉ ghi log — chỗ để thay bằng runtime thật.
+ *
+ * Trong hệ thống thật, dispatch() dựng yêu cầu di chuyển từ mission rồi giao cho navigation runtime,
+ * và runtime báo ngược kết quả qua navDoneEvent(mission->id) / navFailedEvent(mission->id).
+ */
+class LoggingNavigationClient : public mission_adapters::NavigationClient
+{
+public:
+    bool dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission) override
+    {
+        if (!mission)
+            return false;
+
+        robot::log_info("dispatch mission %lu — %zu action",
+                        static_cast<unsigned long>(mission->id),
+                        mission->actions.size());
+        return true;
+    }
+
+    void cancelActive(mission_adapters::MissionId id) override
+    {
+        robot::log_info("cancel mission %lu", static_cast<unsigned long>(id));
+    }
+};
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    robot::init(argc, argv, "mission_adapters_example");
+
+    // Nguồn mission được nạp từ YAML: thêm loại nguồn mới không phải sửa file này.
+    mission_adapters::PluginRegistry registry;
+    robot::NodeHandle nh;
+    if (!registry.loadFromConfig(nh))
+    {
+        robot::log_error("could not load enough mission sources — check "
+                         "mission_adapters_params.yaml");
+        return 1;
+    }
+
+    mission_adapters::MissionManager  mission_manager;
+    mission_adapters::EventProcessor  event_processor(mission_manager, registry);
+    mission_adapters::MissionExecutor mission_executor(mission_manager);
+
+    LoggingNavigationClient navigation_client;
+    mission_executor.setNavigationClient(&navigation_client);
+
+    event_processor.start();
+    mission_executor.start();
+
+    // Nguồn mission thật (MQTT/OPC-UA/REST) gọi các hàm *Event() này từ thread của nó.
+    robot_geometry_msgs::PoseStamped goal;
+    goal.header.frame_id = "map";
+    goal.pose.position.x = 2.0;   // [m]
+    goal.pose.position.y = 1.0;   // [m]
+    event_processor.goalEvent(goal);
+
+    robot::Rate rate(20);   // [Hz]
+    while (robot::ok())
+    {
+        // Runtime thật gọi navDoneEvent(id) / navFailedEvent(id) ở đây, với id lấy từ chính mission
+        // mà dispatch() đã nhận — không suy đoán từ trạng thái navigation.
+        rate.sleep();
+    }
+
+    mission_executor.stop();
+    event_processor.stop();
+    return 0;
+}

examples/vda5050_order_demo.cpp

@@ -0,0 +1,142 @@
+/**
+ * @file vda5050_order_demo.cpp
+ * @brief Chạy một VDA5050 Order qua VDA5050SourceAdapter và in ra các chặng thu được.
+ *
+ * Order dùng ở đây lấy nguyên toạ độ từ một order thật do fleet manager gửi xuống (4 node, 3 edge,
+ * edge cuối có NURBS bậc 2), chỉ khác một điểm: gắn thêm một action vào **node thứ 2**. Mục đích là
+ * thấy tận mắt adapter cắt order thành chặng như thế nào khi có action ở giữa đường.
+ *
+ * Chạy:  ./devel/lib/mission_adapters/vda5050_order_demo
+ */
+#include "mission_adapters/mission_request.h"
+#include "mission_adapters/types.h"
+#include "../plugins/vda5050_source_adapter.h"
+
+#include <cstdio>
+#include <string>
+
+namespace
+{
+
+/// Một node released, kèm toạ độ thật lấy từ order của fleet.
+robot_protocol_msgs::Node makeNode(int sequence_id, const std::string& id, double x, double y)
+{
+    robot_protocol_msgs::Node node;
+    node.sequenceId = sequence_id;
+    node.nodeId = id;
+    node.released = true;
+    node.nodePosition.x = x;
+    node.nodePosition.y = y;
+    node.nodePosition.mapId = "97b3bf93-10c5-4c79-2cc9-08deee20817a";
+    return node;
+}
+
+robot_protocol_msgs::Edge makeEdge(int sequence_id, const std::string& id,
+                                   const std::string& start_id, const std::string& end_id)
+{
+    robot_protocol_msgs::Edge edge;
+    edge.sequenceId = sequence_id;
+    edge.edgeId = id;
+    edge.released = true;
+    edge.startNodeId = start_id;
+    edge.endNodeId = end_id;
+    return edge;
+}
+
+robot_protocol_msgs::Action makeAction(const std::string& id, const std::string& type)
+{
+    robot_protocol_msgs::Action action;
+    action.actionId = id;
+    action.actionType = type;
+    action.blockingType = "HARD";
+    return action;
+}
+
+void printOrder(const robot_protocol_msgs::Order& order)
+{
+    std::printf("ORDER GUI XUONG  (orderId=%s, orderUpdateId=%d)\n",
+                order.orderId.c_str(), (int)order.orderUpdateId);
+    for (const auto& n : order.nodes)
+        std::printf("   node seq=%-2d %-10s pos=(%7.3f, %7.3f)  actions=%zu%s\n",
+                    (int)n.sequenceId, n.nodeId.c_str(),
+                    n.nodePosition.x, n.nodePosition.y, n.actions.size(),
+                    n.actions.empty() ? "" : ("  <-- " + n.actions.front().actionType).c_str());
+    for (const auto& e : order.edges)
+        std::printf("   edge seq=%-2d %-10s %s -> %s  actions=%zu\n",
+                    (int)e.sequenceId, e.edgeId.c_str(),
+                    e.startNodeId.c_str(), e.endNodeId.c_str(), e.actions.size());
+}
+
+void printMissions(const std::vector<std::shared_ptr<mission_adapters::Mission>>& missions)
+{
+    std::printf("\nKET QUA: adapter cat thanh %zu chang\n", missions.size());
+    std::printf("%s\n", std::string(78, '-').c_str());
+
+    int idx = 0;
+    for (const auto& m : missions)
+    {
+        std::printf("  Chang #%d\n", ++idx);
+        std::printf("      type      : %s\n",
+                    m->type == mission_adapters::MissionType::VDA5050_ORDER ? "VDA5050_ORDER"
+                                                                            : "SIMPLE_GOAL");
+        std::printf("      has_goal  : %s\n", m->has_goal ? "true" : "false");
+        if (m->has_goal)
+            std::printf("      di chuyen : (%7.3f, %7.3f) -> (%7.3f, %7.3f)\n",
+                        m->start.pose.position.x, m->start.pose.position.y,
+                        m->goal.pose.position.x, m->goal.pose.position.y);
+        else
+            std::printf("      di chuyen : (khong - chang chi-co-action)\n");
+
+        std::printf("      nodes     : %zu, edges: %zu\n", m->nodes.size(), m->edges.size());
+        if (m->actions.empty())
+        {
+            std::printf("      actions   : (khong co)\n");
+        }
+        else
+        {
+            for (const auto& a : m->actions)
+                std::printf("      actions   : seq=%d  %-12s  id=%s  (%s)\n",
+                            a.sequenceId, a.action.actionType.c_str(), a.action.actionId.c_str(),
+                            a.type == mission_adapters::ActionType::NODE_ACTION ? "NODE_ACTION"
+                                                                                : "EDGE_ACTION");
+        }
+        std::printf("\n");
+    }
+}
+
+}  // namespace
+
+int main()
+{
+    // Toa do lay tu order that: 14:39:21, orderId c16cd9e3..., 4 node / 3 edge.
+    robot_protocol_msgs::Order order;
+    order.orderId = "demo-dua-tren-order-that";
+    order.orderUpdateId = 0;
+
+    order.nodes.push_back(makeNode(0, "node_A", 10.010, 10.000));
+    order.nodes.push_back(makeNode(2, "node_B", 10.260, 10.192));
+    order.nodes.push_back(makeNode(4, "node_C", 14.979, 7.599));
+    order.nodes.push_back(makeNode(6, "node_D", 12.986, 2.851));
+
+    // ĐIỂM KHÁC DUY NHẤT so với order thật: mot action gan vao NODE THU 2.
+    order.nodes[1].actions.push_back(makeAction("act-tai-node-2", "PickUp"));
+
+    order.edges.push_back(makeEdge(1, "edge_AB", "node_A", "node_B"));
+    order.edges.push_back(makeEdge(3, "edge_BC", "node_B", "node_C"));
+    order.edges.push_back(makeEdge(5, "edge_CD", "node_C", "node_D"));
+
+    printOrder(order);
+
+    mission_plugins::VDA5050SourceAdapter adapter;
+
+    std::string reason;
+    const auto request = mission_adapters::MissionRequest::fromOrder(order);
+    if (!adapter.validate(request, reason))
+    {
+        std::printf("\nAdapter TU CHOI order: %s\n", reason.c_str());
+        return 1;
+    }
+
+    printMissions(adapter.convert(request).missions);
+    return 0;
+}

include/mission_adapters/adapter.h

@@ -0,0 +1,95 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Interface plugin duy nhất của gói: nguồn mission.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_ADAPTER_H_
+#define MISSION_ADAPTERS_ADAPTER_H_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <mission_adapters/mission_request.h>
+#include <mission_adapters/types.h>
+
+namespace mission_adapters
+{
+    /// @brief Quan hệ giữa mission mới và hàng đợi đang có.
+    enum class SubmitMode
+    {
+        /// Yêu cầu mới: thay toàn bộ hàng đợi, chặng đang chạy bị preempt (và được bảo dừng).
+        kReplace,
+
+        /**
+         * Phần nối tiếp của yêu cầu đang chạy: chỉ thêm vào cuối hàng đợi.
+         *
+         * Đây là hình dạng của VDA5050 order update — phần horizon được release thêm. Xử lý nó như
+         * kReplace sẽ huỷ và chạy lại chặng đang đi, dù tuyến đường không hề đổi.
+         */
+        kAppend
+    };
+
+    /// @brief Kết quả chuyển đổi: mission sinh ra và cách chúng vào hàng đợi.
+    struct ConversionResult
+    {
+        std::vector<std::shared_ptr<Mission>> missions;
+        SubmitMode mode = SubmitMode::kReplace;
+
+        bool empty() const { return missions.empty(); }
+    };
+
+    /**
+     * @class MissionSourceAdapter
+     * @brief Biến payload của một nguồn cụ thể thành mission của mission layer.
+     *
+     * Plugin được nạp bằng Boost.DLL theo khoá `library_path` trong YAML, giống mọi plugin khác của
+     * workspace. Contract này là public interface qua ranh giới .so: đổi chữ ký thì phải đổi cả
+     * registry trong cùng một lần sửa, vì import_alias là dlsym + reinterpret_cast và không có kiểm
+     * kiểu nào bắt được sự lệch đó lúc biên dịch.
+     *
+     * Việc thực thi action KHÔNG thuộc về interface này: adapter chỉ chuyển dữ liệu, navigation
+     * runtime mới là nơi chạy action (D8).
+     */
+    class MissionSourceAdapter
+    {
+    public:
+        using Ptr = std::shared_ptr<MissionSourceAdapter>;
+
+        virtual ~MissionSourceAdapter() = default;
+
+        /**
+         * @brief Đọc param riêng của instance. Gọi đúng một lần khi nạp.
+         * @param name Tên instance trong `mission_sources` — cũng là namespace param của nó.
+         * @param nh   NodeHandle gốc.
+         * @return false nếu cấu hình không hợp lệ; registry sẽ báo lỗi và không đăng ký adapter này.
+         */
+        virtual bool configure(const std::string& name, robot::NodeHandle& nh) = 0;
+
+        /// @brief Schema mà adapter này xử lý. Core định tuyến theo đúng giá trị này.
+        virtual std::string schema() const = 0;
+
+        /**
+         * @brief Kiểm payload TRƯỚC khi convert.
+         * @param[out] reason Lý do từ chối, dùng cho log.
+         * @return false -> core từ chối sớm và không đụng vào hàng đợi.
+         */
+        virtual bool validate(const MissionRequest& request, std::string& reason) const = 0;
+
+        /**
+         * @brief Chuyển payload thành 0..n mission, kèm cách chúng vào hàng đợi.
+         *
+         * Trả rỗng nghĩa là **không có việc**, KHÔNG phải lỗi và cũng không phải lệnh huỷ: core sẽ
+         * bỏ qua, giữ nguyên hàng đợi đang chạy (A1).
+         */
+        virtual ConversionResult convert(const MissionRequest& request) = 0;
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_ADAPTER_H_

include/mission_adapters/event.h

@@ -0,0 +1,121 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Sự kiện của mission layer và hàng đợi vận chuyển chúng.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_EVENT_H_
+#define MISSION_ADAPTERS_EVENT_H_
+
+#include <atomic>
+#include <condition_variable>
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <queue>
+#include <vector>
+
+#include <mission_adapters/mission_request.h>
+#include <mission_adapters/types.h>
+
+namespace mission_adapters
+{
+    enum class EventType
+    {
+        SUBMIT_REQUEST,
+        NAV_DONE,
+        NAV_FAILED,
+        PAUSE,
+        RESUME,
+        CANCEL,
+        EMERGENCY,
+        CLEAR_EMERGENCY
+    };
+
+    struct Event
+    {
+        EventType type = EventType::SUBMIT_REQUEST;
+        uint64_t sequence = 0;   ///< thứ tự phát sinh, EventBus tự gán
+
+        /// Chỉ dùng cho NAV_DONE / NAV_FAILED: mission nào vừa kết thúc.
+        MissionId mission_id = kInvalidMissionId;
+
+        /// Chỉ dùng cho SUBMIT_REQUEST: payload chưa chuyển đổi, adapter sẽ xử lý trên thread worker.
+        MissionRequest request;
+    };
+
+    /**
+     * @class EventBus
+     * @brief Hàng đợi sự kiện FIFO, cộng một đường out-of-band riêng cho emergency.
+     *
+     * **FIFO là bắt buộc, không phải lựa chọn phong cách.** Bản trước sắp xếp theo priority toàn
+     * phần nên CANCEL (ưu tiên cao) luôn được xử lý trước SUBMIT (ưu tiên thấp) — phát order rồi huỷ
+     * ngay thì huỷ chạy trước, order vào hàng đợi sau và robot chạy đúng cái người dùng vừa huỷ.
+     * Thứ tự xử lý phải bằng thứ tự phát sinh thì quan hệ nhân quả mới còn.
+     *
+     * Emergency không đi theo luật đó: nó bật một cờ atomic ngay tại chỗ gọi, nên độ trễ phản ứng
+     * không phụ thuộc vào việc hàng đợi đang dài bao nhiêu. Sự kiện EMERGENCY vẫn được đẩy vào hàng
+     * đợi để dòng thời gian trong log không bị thủng.
+     */
+    class EventBus
+    {
+    public:
+        /// @brief Đẩy một sự kiện thường vào cuối hàng đợi.
+        void push(Event event);
+
+        /**
+         * @brief Đẩy sự kiện emergency: bật cờ out-of-band trước, rồi mới xếp hàng.
+         *
+         * Bên tiêu thụ phải gọi @ref takeEmergency trước khi xử lý sự kiện tiếp theo.
+         */
+        void pushEmergency(Event event);
+
+        /// @brief Lấy sự kiện kế tiếp, chờ nếu hàng đợi rỗng. Trả false khi bus đã stop().
+        bool pop(Event& event);
+
+        /**
+         * @brief Lấy và xoá cờ emergency đang treo.
+         * @return true nếu có emergency chưa được xử lý.
+         */
+        bool takeEmergency();
+
+        /// @brief Có emergency đang treo hay không (không xoá cờ).
+        bool emergencyPending() const;
+
+        void stop();
+        void reset();
+
+        bool empty() const
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            return queue_.empty();
+        }
+
+        size_t size() const
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            return queue_.size();
+        }
+
+        void clear()
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            queue_ = {};
+        }
+
+    private:
+        mutable std::mutex mutex_;
+        std::queue<Event> queue_;
+        std::condition_variable cv_;
+        bool stop_ = false;
+        uint64_t next_sequence_ = 0;
+
+        /// Ngoài hàng đợi có chủ đích: emergency không được xếp sau một hàng dài sự kiện thường.
+        std::atomic<bool> emergency_pending_{false};
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_EVENT_H_

include/mission_adapters/event_processor.h

@@ -0,0 +1,93 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Thread tiêu thụ event: biến sự kiện từ host thành lời gọi lên MissionManager.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_EVENT_PROCESSOR_H_
+#define MISSION_ADAPTERS_EVENT_PROCESSOR_H_
+
+#include <atomic>
+#include <thread>
+
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_protocol_msgs/Order.h>
+
+#include <mission_adapters/event.h>
+#include <mission_adapters/mission_manager.h>
+#include <mission_adapters/mission_request.h>
+#include <mission_adapters/plugin_registry.h>
+
+namespace mission_adapters
+{
+    /**
+     * @class EventProcessor
+     * @brief Thread duy nhất biến sự kiện thành thay đổi trạng thái mission.
+     *
+     * Core không biết adapter nào tồn tại: nó chỉ tra @ref PluginRegistry theo
+     * MissionRequest::schema. Thêm một loại nguồn mới = thêm plugin + một dòng YAML, không sửa file
+     * nào trong `src/` của gói.
+     *
+     * Việc chuyển payload -> mission chạy trên đúng thread này (không phải thread của host) vì
+     * adapter được phép có state; chuyển đổi trên nhiều thread host sẽ tranh chấp state đó.
+     */
+    class EventProcessor
+    {
+    public:
+        EventProcessor(MissionManager& mission_manager, PluginRegistry& registry);
+        ~EventProcessor();
+
+        void start();
+        void stop();
+
+        /**
+         * @brief Đường vào chung cho mọi nguồn mission.
+         *
+         * Nguồn mới chỉ cần dựng MissionRequest với schema của mình rồi gọi hàm này.
+         */
+        void submitRequest(const MissionRequest& request);
+
+        /// @brief Tiện ích cho nguồn goal đơn lẻ. Chỉ dựng envelope, không biết adapter nào xử lý.
+        void goalEvent(const robot_geometry_msgs::PoseStamped& goal);
+
+        /// @brief Tiện ích cho nguồn VDA5050.
+        void orderEvent(const robot_protocol_msgs::Order& order);
+
+        void cancelEvent();
+        void pauseEvent();
+        void resumeEvent();
+
+        /**
+         * @brief Báo một chặng đã hoàn tất (nav + action — D8).
+         * @param id Mission vừa xong. Bắt buộc: outcome không mang ID sẽ bị manager loại bỏ.
+         */
+        void navDoneEvent(MissionId id);
+
+        /// @brief Báo một chặng thất bại. Cũng bắt buộc mang MissionId.
+        void navFailedEvent(MissionId id);
+
+        void emergencyEvent();
+        void clearEmergencyEvent();
+
+    private:
+        void spin();
+        void process(const Event& event);
+
+        /// Tra adapter theo schema, validate, convert, rồi submit nếu có mission.
+        void handleRequest(const MissionRequest& request);
+
+        /// Xử lý emergency đang treo (nếu có) trước khi động tới hàng đợi sự kiện thường.
+        void handlePendingEmergency();
+
+        EventBus event_bus_;
+        MissionManager& mission_manager_;
+        PluginRegistry& registry_;
+        std::thread worker_;
+        std::atomic<bool> running_{false};
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_EVENT_PROCESSOR_H_

include/mission_adapters/mission_adapters.h

@@ -1,245 +1,25 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Header façade — gộp toàn bộ public API của gói.
+ *
+ * Giữ lại để code đã include header này không phải sửa. Code mới nên include thẳng header cần dùng
+ * (types.h / event.h / adapter.h / mission_manager.h / event_processor.h / mission_executor.h).
+ *
+ * Author: DuongTD
+ *********************************************************************/
 #ifndef MISSION_ADAPTERS_MISSION_ADAPTERS_H
 #define MISSION_ADAPTERS_MISSION_ADAPTERS_H
 
-#include <string>
-#include <vector>
-#include <queue>
-#include <memory>
-#include <thread>
-#include <mutex>
-#include <condition_variable>
-#include <functional>
-#include <atomic>
-#include <cstdint>
-
 #include <robot/robot.h>
-#include <robot_protocol_msgs/Order.h>
-#include <robot_geometry_msgs/PoseStamped.h>
 
-namespace mission_adapters
+#include <mission_adapters/adapter.h>
-{
+#include <mission_adapters/event.h>
-    // ── Priority constants ────────────────────────────────────────────────────
+#include <mission_adapters/event_processor.h>
-    // Số càng nhỏ = ưu tiên càng cao (min-heap)
+#include <mission_adapters/mission_executor.h>
-    static constexpr int PRIORITY_EMERGENCY = 0;
+#include <mission_adapters/mission_manager.h>
-    static constexpr int PRIORITY_CANCEL    = 1;
+#include <mission_adapters/navigation_client.h>
-    static constexpr int PRIORITY_RESUME    = 2;
+#include <mission_adapters/types.h>
-    static constexpr int PRIORITY_PAUSE     = 3;
-    static constexpr int PRIORITY_NAV_DONE  = 4;
-    // FIX #4: Dedicated constant — same value as NAV_DONE but semantically correct name.
-    static constexpr int PRIORITY_NAV_FAILED = 4;
-    static constexpr int PRIORITY_ORDER     = 5;
-
-    enum class MissionState
-    {
-        IDLE,
-        QUEUED,
-        RUNNING,
-        PAUSED,
-        RECOVERY,
-        COMPLETED,
-        FAILED,
-        CANCELLED,
-        EMERGENCY,
-        CLEAR_EMERGENCY
-    };
-
-    enum class EventType
-    {
-        SUBMIT_MISSIONS,
-        NAV_DONE,
-        NAV_FAILED,
-        PAUSE,
-        RESUME,
-        CANCEL,
-        EMERGENCY,
-        CLEAR_EMERGENCY
-    };
-
-    enum class MissionType
-    {
-        SIMPLE_GOAL,
-        VDA5050_ORDER
-    };
-
-    enum class ActionType
-    {
-        NODE_ACTION,
-        EDGE_ACTION
-    };
-
-    class Action
-    {
-    public:
-        int sequenceId = 0;
-        ActionType type = ActionType::NODE_ACTION;
-        robot_protocol_msgs::Action action;
-    };
-
-    class Mission
-    {
-    public:
-        int sequenceId = 0;
-        MissionType type = MissionType::SIMPLE_GOAL;
-        int priority = 0;
-        robot_geometry_msgs::PoseStamped start;
-        robot_geometry_msgs::PoseStamped goal;
-        std::vector<robot_protocol_msgs::Node> nodes;
-        std::vector<robot_protocol_msgs::Edge> edges;
-        std::vector<Action> actions;
-    };
-
-    struct Event
-    {
-        EventType type = EventType::SUBMIT_MISSIONS;
-        int priority = PRIORITY_ORDER;
-        uint64_t sequence = 0;
-        std::vector<std::shared_ptr<Mission>> missions;
-    };
-
-    struct EventCompare
-    {
-        bool operator()(const Event& lhs, const Event& rhs) const
-        {
-            if (lhs.priority != rhs.priority)
-                return lhs.priority > rhs.priority;
-
-            return lhs.sequence > rhs.sequence;
-        }
-    };
-
-    class EventBus
-    {
-    private:
-        mutable std::mutex mutex_;
-        std::priority_queue<Event, std::vector<Event>, EventCompare> queue_;
-        std::condition_variable cv_;
-        bool stop_ = false;
-        uint64_t next_sequence_ = 0;
-
-    public:
-        void push(const Event& event);
-        void push(Event&& event);
-        bool pop(Event& event);
-        void stop();
-        void reset();
-
-        inline bool empty() const
-        {
-            std::lock_guard<std::mutex> lock(mutex_);
-            return queue_.empty();
-        }
-
-        inline void clear()
-        {
-            std::lock_guard<std::mutex> lock(mutex_);
-            while (!queue_.empty()) queue_.pop();
-        }
-    };
-
-    class VDA5050Adapter
-    {
-    public:
-        std::vector<std::shared_ptr<Mission>> convert(const robot_protocol_msgs::Order& order);
-    };
-
-    class GoalAdapter
-    {
-    public:
-        std::vector<std::shared_ptr<Mission>> convert(const robot_geometry_msgs::PoseStamped& goal);
-    };
-
-    class MissionManager
-    {
-    public:
-        void submit(const std::vector<std::shared_ptr<Mission>>& missions);
-
-        std::shared_ptr<Mission> nextMission();
-
-        void onNavigationDone();
-        void onNavigationFailed();
-        void cancel();
-        void pause();
-        void resume();
-        void emergency();
-        void clearEmergency();
-
-        MissionState state() const;
-
-        // FIX #3: Returns true if there is any pending work — queue OR active mission.
-        bool hasMission() const;
-
-    private:
-        mutable std::mutex mutex_;
-        MissionState state_ = MissionState::IDLE;
-        std::queue<std::shared_ptr<Mission>> mission_queue_;
-        std::shared_ptr<Mission> current_mission_;
-    };
-
-    class EventProcessor
-    {
-    public:
-        explicit EventProcessor(MissionManager& mission_manager);
-        ~EventProcessor();
-
-        void start();
-        void stop();
-
-        void goalEvent(const robot_geometry_msgs::PoseStamped& goal);
-        void orderEvent(const robot_protocol_msgs::Order& order);
-        void cancelEvent();
-        void pauseEvent();
-        void resumeEvent();
-        void navDoneEvent();
-        void navFailedEvent();
-        void emergencyEvent();
-        void clearEmergencyEvent();
-
-    private:
-        void spin();
-        void process(const Event& event);
-
-    private:
-        EventBus event_bus_;
-        GoalAdapter goal_adapter_;
-        VDA5050Adapter vda5050_adapter_;
-        MissionManager& mission_manager_;
-        std::thread worker_;
-        std::atomic<bool> running_{false};
-    };
-
-    class MissionExecutor
-    {
-    public:
-        using MissionCallback = std::function<void(const std::shared_ptr<Mission>&)>;
-
-        explicit MissionExecutor(MissionManager& manager);
-        ~MissionExecutor();
-
-        void start();
-        void stop();
-
-        // FIX #1: Must be called BEFORE start(), not inside the run loop.
-        void setMissionCallback(MissionCallback cb)
-        {
-            std::lock_guard<std::mutex> lock(mutex_);
-            mission_callback_ = std::move(cb);
-        }
-
-    private:
-        std::shared_ptr<Mission> mission_execute_;
-
-        // Last mission that was dispatched to the callback.
-        std::shared_ptr<Mission> last_dispatched_mission_;
-
-        MissionCallback mission_callback_;
-        MissionManager& mission_manager_;
-        mutable std::mutex mutex_;
-        std::thread worker_;
-        std::atomic<bool> running_{false};
-
-        void spin();
-    };
-
-} // namespace mission_adapters
 
 #endif  // MISSION_ADAPTERS_MISSION_ADAPTERS_H

include/mission_adapters/mission_config.h

@@ -0,0 +1,64 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Tham số vận hành của mission layer.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_MISSION_CONFIG_H_
+#define MISSION_ADAPTERS_MISSION_CONFIG_H_
+
+#include <string>
+
+#include <robot/node_handle.h>
+
+namespace mission_adapters
+{
+    /**
+     * @struct MissionConfig
+     * @brief Tham số vận hành, nạp từ `mission_adapters_params.yaml`.
+     *
+     * Bản runtime của file nằm ở `pnkx_nav_core/config/`; bản trong `test/config/` chỉ dùng cho test
+     * và phải chạy kèm PNKX_NAV_CORE_CONFIG_DIR trỏ vào đó.
+     */
+    struct MissionConfig
+    {
+        /**
+         * Trần thời gian cho MỘT chặng (nav + action), tính từ lúc chặng được giao. [s]
+         *
+         * 0 = tắt. Quá hạn thì chặng bị đánh dấu thất bại và navigation được bảo dừng — lưới cuối
+         * cho trường hợp navigation không bao giờ báo kết quả về.
+         */
+        double mission_timeout = 0.0;
+
+        /**
+         * Một chặng thất bại thì xử lý phần còn lại của hàng đợi thế nào.
+         *
+         * `true` (mặc định) = xoá sạch hàng đợi. Đây là hành vi an toàn cho tuyến đường tuần tự
+         * kiểu VDA5050: chặng 2 hỏng nghĩa là robot không tới được node 2, nên chạy tiếp chặng 3 là
+         * cắt ngang qua đoạn đường mà fleet manager chưa cho phép đi.
+         *
+         * `false` = chỉ bỏ chặng lỗi rồi chạy tiếp chặng sau. Chỉ đặt false khi các mission trong
+         * hàng đợi ĐỘC LẬP với nhau (nhiều goal rời rạc), không phải các chặng của cùng một tuyến.
+         */
+        bool clear_queue_on_failure = true;
+
+        /**
+         * @brief Nạp tham số từ namespace @p ns.
+         * @return false nếu giá trị nạp được không qua @ref validate.
+         *
+         * Thiếu khoá không phải lỗi: giá trị mặc định trong struct này là hợp lệ và an toàn.
+         */
+        bool loadFromParams(robot::NodeHandle& nh, const std::string& ns = "mission_adapters");
+
+        /// @brief Kiểm miền giá trị. Trả false kèm log nêu đích danh tham số sai.
+        bool validate() const;
+
+        /// @brief In cấu hình đang dùng — để log khởi động nói được vì sao robot hành xử như vậy.
+        void print() const;
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_MISSION_CONFIG_H_

include/mission_adapters/mission_executor.h

@@ -0,0 +1,62 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Thread lấy mission kế tiếp từ MissionManager và đẩy xuống phía navigation.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_MISSION_EXECUTOR_H_
+#define MISSION_ADAPTERS_MISSION_EXECUTOR_H_
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#include <mission_adapters/mission_manager.h>
+#include <mission_adapters/navigation_client.h>
+#include <mission_adapters/types.h>
+
+namespace mission_adapters
+{
+    /**
+     * @class MissionExecutor
+     * @brief Thread duy nhất phát lệnh xuống navigation.
+     *
+     * Mọi lời gọi ra ngoài (dispatch, cancelActive) đi ra từ đây, nên MissionManager không bao giờ
+     * gọi ra ngoài khi đang giữ khoá. Thứ tự trong một vòng là bất biến an toàn: **cancel trước,
+     * dispatch sau** — chặng cũ phải được bảo dừng trước khi chặng mới bắt đầu.
+     */
+    class MissionExecutor
+    {
+    public:
+        explicit MissionExecutor(MissionManager& manager);
+        ~MissionExecutor();
+
+        /**
+         * @brief Gắn cổng navigation. Con trỏ non-owning, phải sống lâu hơn executor.
+         *
+         * FIX #1: gọi TRƯỚC start(), không gọi trong vòng lặp.
+         */
+        void setNavigationClient(NavigationClient* client);
+
+        void start();
+        void stop();
+
+    private:
+        void spin();
+
+        /// Một vòng công việc. Tách riêng để test gọi trực tiếp, không cần thread.
+        void step();
+
+        NavigationClient* navigation_client_ = nullptr;   ///< non-owning, có thể null
+        MissionManager& mission_manager_;
+        mutable std::mutex mutex_;
+        std::thread worker_;
+        std::atomic<bool> running_{false};
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_MISSION_EXECUTOR_H_

include/mission_adapters/mission_manager.h

@@ -0,0 +1,165 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Trạng thái và hàng đợi mission — nơi duy nhất giữ MissionState.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_MISSION_MANAGER_H_
+#define MISSION_ADAPTERS_MISSION_MANAGER_H_
+
+#include <condition_variable>
+#include <memory>
+#include <mutex>
+#include <queue>
+#include <vector>
+
+#include <robot/time.h>
+
+#include <mission_adapters/mission_config.h>
+#include <mission_adapters/types.h>
+
+namespace mission_adapters
+{
+    /**
+     * @class MissionManager
+     * @brief Hàng đợi mission và trạng thái của nó. Thread-safe, không tự gọi ra ngoài.
+     *
+     * Manager cố ý KHÔNG giữ con trỏ tới navigation: nó chỉ ghi nhận "mission này cần được dừng"
+     * (@ref takePendingCancel) và để MissionExecutor thực hiện lời gọi đó. Nhờ vậy không có lời gọi
+     * ra ngoài nào xảy ra khi đang giữ khoá, và mọi lệnh xuống navigation đi ra từ đúng một thread.
+     */
+    class MissionManager
+    {
+    public:
+        MissionManager() = default;
+        explicit MissionManager(const MissionConfig& config) : config_(config) {}
+
+        /// @brief Đổi tham số vận hành. Gọi trước khi có mission nào đang chạy.
+        void setConfig(const MissionConfig& config);
+
+        /**
+         * @brief Nhận một loạt mission mới và cấp @ref MissionId cho từng cái.
+         *
+         * Semantics (A1): danh sách rỗng là **no-op tuyệt đối** — không đụng hàng đợi, không đổi
+         * trạng thái. Một order lỗi không phải là lệnh huỷ việc đang chạy.
+         *
+         * Submit khi đang RUNNING là **preempt tường minh**: mission đang chạy được ghi vào ô
+         * pending-cancel để navigation được bảo dừng, rồi hàng đợi mới thay hàng đợi cũ.
+         */
+        void submit(const std::vector<std::shared_ptr<Mission>>& missions);
+
+        /**
+         * @brief Nối thêm mission vào cuối hàng đợi, KHÔNG đụng chặng đang chạy.
+         *
+         * Dùng cho phần nối tiếp của một yêu cầu đang chạy (VDA5050 order update release thêm
+         * horizon). Khác @ref submit ở chỗ không preempt và không yêu cầu dừng navigation — tuyến
+         * đường không đổi thì robot không có lý do gì phải dừng lại.
+         */
+        void append(const std::vector<std::shared_ptr<Mission>>& missions);
+
+        /**
+         * @brief Lấy mission kế tiếp cần chạy.
+         * @return Mission MỚI, hoặc nullptr nếu chưa tới lượt.
+         *
+         * Chỉ trả khác null đúng một lần cho mỗi mission, tại đúng bước chuyển QUEUED -> RUNNING
+         * (M6). Bên gọi vì thế không cần tự khử trùng lặp.
+         */
+        std::shared_ptr<const Mission> nextMission();
+
+        /**
+         * @brief Ghi nhận một chặng hoàn tất (cả navigation lẫn action — D8).
+         * @param id Mission vừa xong.
+         * @return false nếu outcome không thuộc mission đang chạy (đến trễ sau khi bị thay thế).
+         */
+        bool onNavigationDone(MissionId id);
+
+        /**
+         * @brief Ghi nhận một chặng thất bại.
+         * @return false nếu outcome không thuộc mission đang chạy.
+         */
+        bool onNavigationFailed(MissionId id);
+
+        void cancel();
+        void pause();
+        void resume();
+        void emergency();
+        void clearEmergency();
+
+        MissionState state() const;
+
+        /// @brief Có việc đang treo hay không — hàng đợi HOẶC mission đang chạy.
+        bool hasMission() const;
+
+        /// @brief Mission đang chạy, kInvalidMissionId nếu không có.
+        MissionId currentMissionId() const;
+
+        /**
+         * @brief Lấy và xoá yêu cầu dừng navigation đang treo.
+         * @return kInvalidMissionId nếu không có yêu cầu nào.
+         *
+         * Chỉ MissionExecutor gọi hàm này; nó là cách manager nhờ executor gọi
+         * NavigationClient::cancelActive() mà bản thân không cần biết navigation là gì.
+         */
+        MissionId takePendingCancel();
+
+        /**
+         * @brief Chờ tới khi có việc cho executor: mission mới cần giao, hoặc yêu cầu dừng đang treo.
+         * @return false nếu bị đánh thức bởi @ref wakeUp (đang dừng), true nếu thật sự có việc.
+         *
+         * Thay cho vòng poll: mission layer không có deadline định kỳ nào, poll chỉ thêm độ trễ
+         * trung bình nửa chu kỳ vào đường phản ứng cancel/emergency.
+         */
+        bool waitForWork();
+
+        /// @brief Đánh thức mọi thread đang chờ trong @ref waitForWork (dùng khi dừng executor).
+        void wakeUp();
+
+    private:
+        /// Gọi khi đã giữ mutex_. Ghi mission đang chạy vào ô pending-cancel rồi bỏ nó ra.
+        void requestCancelOfCurrentLocked();
+
+        /// Gọi khi đã giữ mutex_. Id mission đang chạy, kInvalidMissionId nếu không có.
+        MissionId currentIdLocked() const;
+
+        /**
+         * Gọi khi đã giữ mutex_. Đổi trạng thái và ghi log ĐÚNG MỘT LẦN tại mỗi lần đổi thật.
+         *
+         * Đây là nơi duy nhất được gán state_: nếu không, mission biến mất khỏi hàng đợi mà không
+         * có dòng log nào cho biết ai đã làm điều đó và vì sao.
+         *
+         * @param reason Lý do ngắn gọn, cho người đọc log lúc 2 giờ sáng.
+         */
+        void setStateLocked(MissionState next, const char* reason, MissionId id = kInvalidMissionId);
+
+        /// Gọi khi đã giữ mutex_. true nếu executor có việc để làm ngay.
+        bool hasWorkLocked() const;
+
+        /// Gọi khi đã giữ mutex_. true nếu đang có chặng chạy và mission_timeout được bật.
+        bool timeoutArmedLocked() const;
+
+        /**
+         * Gọi khi đã giữ mutex_. Đánh dấu chặng đang chạy là thất bại do quá hạn, và ghi yêu cầu
+         * dừng navigation — khác NAV_FAILED ở chỗ navigation vẫn đang chạy nên phải bảo nó dừng.
+         */
+        void expireCurrentLocked();
+
+        mutable std::mutex mutex_;
+        std::condition_variable work_cv_;
+        bool wake_requested_ = false;
+        MissionState state_ = MissionState::IDLE;
+        std::queue<std::shared_ptr<const Mission>> mission_queue_;
+        std::shared_ptr<const Mission> current_mission_;
+        MissionId pending_cancel_ = kInvalidMissionId;
+
+        /// Thời điểm chặng hiện tại được giao — mốc tính mission_timeout.
+        robot::Time mission_start_time_;
+
+        MissionConfig config_;
+        MissionId next_mission_id_ = 1;   ///< 0 dành riêng cho kInvalidMissionId
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_MISSION_MANAGER_H_

include/mission_adapters/mission_request.h

@@ -0,0 +1,77 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Envelope cho payload đầu vào của mission layer.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_MISSION_REQUEST_H_
+#define MISSION_ADAPTERS_MISSION_REQUEST_H_
+
+#include <memory>
+#include <string>
+
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_protocol_msgs/Order.h>
+
+namespace mission_adapters
+{
+    /// @brief Tên schema của các nguồn có sẵn. Nguồn mới tự khai schema riêng, không cần sửa ở đây.
+    namespace schema
+    {
+        constexpr const char* kVda5050Order = "vda5050.order";
+        constexpr const char* kPoseStamped  = "geometry.pose_stamped";
+    }  // namespace schema
+
+    /**
+     * @struct MissionRequest
+     * @brief Payload đầu vào cùng với tên schema mô tả nó.
+     *
+     * Hai nguồn hiện có nhận hai kiểu khác hẳn nhau (Order và PoseStamped) nên không thể có một
+     * convert() chung nếu không bọc lại. Core chỉ định tuyến theo @ref schema và không bao giờ cần
+     * biết kiểu thật; thêm nguồn mới = thêm schema mới + plugin mới.
+     *
+     * Payload có kiểu dùng cho nguồn nội bộ (tránh serialize thừa); nguồn ngoài (REST/MQTT/file) đi
+     * bằng @ref raw_payload và tự parse trong plugin của mình.
+     */
+    struct MissionRequest
+    {
+        std::string schema;
+
+        std::shared_ptr<robot_protocol_msgs::Order>       order;
+        std::shared_ptr<robot_geometry_msgs::PoseStamped> pose;
+
+        std::string raw_payload;
+
+        /// @brief Dựng request từ một VDA5050 Order.
+        static MissionRequest fromOrder(const robot_protocol_msgs::Order& order)
+        {
+            MissionRequest request;
+            request.schema = schema::kVda5050Order;
+            request.order  = std::make_shared<robot_protocol_msgs::Order>(order);
+            return request;
+        }
+
+        /// @brief Dựng request từ một goal đơn lẻ.
+        static MissionRequest fromPose(const robot_geometry_msgs::PoseStamped& pose)
+        {
+            MissionRequest request;
+            request.schema = schema::kPoseStamped;
+            request.pose   = std::make_shared<robot_geometry_msgs::PoseStamped>(pose);
+            return request;
+        }
+
+        /// @brief Dựng request từ payload thô của một nguồn ngoài.
+        static MissionRequest fromRaw(const std::string& schema_name, const std::string& payload)
+        {
+            MissionRequest request;
+            request.schema      = schema_name;
+            request.raw_payload = payload;
+            return request;
+        }
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_MISSION_REQUEST_H_

include/mission_adapters/navigation_client.h

@@ -0,0 +1,55 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Cổng ra phía navigation của mission layer.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_NAVIGATION_CLIENT_H_
+#define MISSION_ADAPTERS_NAVIGATION_CLIENT_H_
+
+#include <memory>
+
+#include <mission_adapters/types.h>
+
+namespace mission_adapters
+{
+    /**
+     * @class NavigationClient
+     * @brief Đường duy nhất để mission layer điều khiển navigation.
+     *
+     * Gói này KHÔNG biết navigation runtime nào đang chạy phía sau: bridge phía runtime hiện thực
+     * interface này, test hiện thực bằng fake. Nhờ vậy chiều phụ thuộc luôn một chiều và mission
+     * layer test được mà không cần robot.
+     *
+     * Bất biến an toàn: mọi đường thoát của một mission — bị thay thế, bị huỷ, emergency — đều phải
+     * đi qua @ref cancelActive. Mission layer quên mission mà không bảo navigation dừng thì robot
+     * vẫn chạy tiếp tới goal cũ.
+     */
+    class NavigationClient
+    {
+    public:
+        virtual ~NavigationClient() = default;
+
+        /**
+         * @brief Giao một mission cho navigation chạy.
+         * @param mission Mission bất biến, luôn khác null.
+         * @return false nếu navigation từ chối (goal không hợp lệ, chưa initialize...). Khi đó
+         *         mission layer ghi nhận chặng này thất bại thay vì chờ vô hạn.
+         */
+        virtual bool dispatch(const std::shared_ptr<const Mission>& mission) = 0;
+
+        /**
+         * @brief Yêu cầu dừng mission đang chạy.
+         * @param id Mission cần dừng — navigation bỏ qua nếu nó đã chạy sang mission khác.
+         *
+         * Kết quả cuối cùng vẫn quay về qua đường outcome bình thường (navDoneEvent /
+         * navFailedEvent mang cùng @p id), không phải qua giá trị trả về của hàm này.
+         */
+        virtual void cancelActive(MissionId id) = 0;
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_NAVIGATION_CLIENT_H_

include/mission_adapters/plugin_registry.h

@@ -0,0 +1,94 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Nạp và tra cứu MissionSourceAdapter theo schema.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_PLUGIN_REGISTRY_H_
+#define MISSION_ADAPTERS_PLUGIN_REGISTRY_H_
+
+#include <functional>
+#include <map>
+#include <string>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <mission_adapters/adapter.h>
+
+namespace mission_adapters
+{
+    /**
+     * @class PluginRegistry
+     * @brief Bảng tra schema -> adapter, nạp từ YAML bằng Boost.DLL.
+     *
+     * Cấu hình mong đợi (xem `pnkx_nav_core/config/mission_adapters_params.yaml`):
+     *
+     * @code{.yaml}
+     * mission_adapters:
+     *   mission_sources:
+     *     - {name: goal_src,    type: GoalSourceAdapter}
+     *     - {name: vda5050_src, type: VDA5050SourceAdapter}
+     *
+     * GoalSourceAdapter:
+     *   library_path: libmission_adapters_goal_source
+     * @endcode
+     *
+     * Khoá `library_path` là thứ hay bị quên nhất: thiếu nó thì plugin build xong vẫn báo "không
+     * tìm thấy" lúc chạy. Registry vì thế báo lỗi nêu đích danh khoá bị thiếu thay vì chỉ nói
+     * không nạp được.
+     */
+    class PluginRegistry
+    {
+    public:
+        PluginRegistry() = default;
+        ~PluginRegistry();
+
+        PluginRegistry(const PluginRegistry&) = delete;
+        PluginRegistry& operator=(const PluginRegistry&) = delete;
+
+        /**
+         * @brief Nạp toàn bộ nguồn khai trong `<ns>/mission_sources`.
+         * @param nh NodeHandle gốc.
+         * @param ns Namespace chứa danh sách nguồn.
+         * @return false nếu có bất kỳ nguồn nào không nạp được. Các nguồn còn lại vẫn được đăng ký,
+         *         và mỗi lỗi được log kèm lý do cụ thể.
+         */
+        bool loadFromConfig(robot::NodeHandle& nh, const std::string& ns = "mission_adapters");
+
+        /**
+         * @brief Đăng ký một adapter đã dựng sẵn (test, hoặc nguồn biên dịch thẳng vào host).
+         * @return false nếu adapter null, schema rỗng, hoặc schema đã có chủ.
+         */
+        bool registerAdapter(const MissionSourceAdapter::Ptr& adapter);
+
+        /// @brief Tìm adapter xử lý schema này. nullptr nếu không có.
+        MissionSourceAdapter* find(const std::string& schema) const;
+
+        size_t size() const { return adapters_.size(); }
+
+        /// @brief Danh sách schema đã đăng ký, dùng cho log và test.
+        std::vector<std::string> schemas() const;
+
+        void clear();
+
+    private:
+        /// Nạp một nguồn. Trả false kèm log lý do nếu hỏng ở bất kỳ bước nào.
+        bool loadOne(const std::string& name, const std::string& type, robot::NodeHandle& nh);
+
+        std::map<std::string, MissionSourceAdapter::Ptr> adapters_;
+
+        /**
+         * Giữ factory của Boost.DLL sống đúng bằng vòng đời registry.
+         *
+         * Đây 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 các adapter tạo từ nó vẫn còn sống — vtable trỏ vào vùng nhớ đã gỡ.
+         */
+        std::vector<std::function<MissionSourceAdapter::Ptr()>> factories_;
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_PLUGIN_REGISTRY_H_

include/mission_adapters/types.h

@@ -0,0 +1,144 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Kiểu dữ liệu nền của mission layer: trạng thái, mission, action.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_TYPES_H_
+#define MISSION_ADAPTERS_TYPES_H_
+
+#include <cstdint>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_protocol_msgs/Order.h>
+
+namespace mission_adapters
+{
+    /**
+     * @brief Danh tính của một mission, do MissionManager cấp khi submit và đơn điệu tăng.
+     *
+     * Mọi kết quả từ phía navigation phải mang ID này. Không có nó thì outcome trễ của một mission
+     * đã bị thay thế sẽ được ghi nhận cho mission đang chạy — mission mới "hoàn thành" mà robot
+     * chưa hề đi.
+     */
+    using MissionId = std::uint64_t;
+
+    /// @brief Mission chưa được submit (hoặc không tồn tại).
+    static constexpr MissionId kInvalidMissionId = 0;
+
+    /// @brief Trạng thái hàng đợi mission. Tại mỗi thời điểm robot chỉ ở đúng một trạng thái.
+    enum class MissionState
+    {
+        IDLE,
+        QUEUED,
+        RUNNING,
+        PAUSED,
+        COMPLETED,
+        FAILED,
+        CANCELLED,
+        EMERGENCY,
+        CLEAR_EMERGENCY
+    };
+
+    /// @brief Tên trạng thái, dùng cho log và cho host hiển thị.
+    const char* toString(MissionState state);
+
+    /// @brief Nguồn gốc mission, quyết định cách consumer diễn giải nodes/edges.
+    enum class MissionType
+    {
+        SIMPLE_GOAL,
+        VDA5050_ORDER
+    };
+
+    /// @brief Action gắn với node (thực hiện tại chỗ) hay với edge (thực hiện dọc đường đi).
+    enum class ActionType
+    {
+        NODE_ACTION,
+        EDGE_ACTION
+    };
+
+    class Action
+    {
+    public:
+        int sequenceId = 0;
+        ActionType type = ActionType::NODE_ACTION;
+        robot_protocol_msgs::Action action;
+    };
+
+    /**
+     * @brief Một chặng việc self-contained mà navigation runtime có thể chạy trọn vẹn.
+     *
+     * @invariant Bất biến sau khi MissionManager::submit() gán @ref id: từ thời điểm đó mission chỉ
+     *            được chia sẻ dưới dạng std::shared_ptr<const Mission>, nên ba thread (event, exec,
+     *            navigation) đọc chung mà không cần khoá.
+     */
+    class Mission
+    {
+    public:
+        MissionId id = kInvalidMissionId;   ///< 0 cho tới khi submit; sau đó không đổi
+        MissionType type = MissionType::SIMPLE_GOAL;
+
+        /**
+         * false = mission chỉ-có-action: navigation runtime bỏ qua phần di chuyển và vào thẳng
+         * thực thi action (D8). @ref start và @ref goal khi đó không mang ý nghĩa.
+         *
+         * @invariant has_goal == false thì @ref actions phải khác rỗng — mission không goal cũng
+         *            không action là mission không có việc gì để làm.
+         */
+        bool has_goal = true;
+
+        robot_geometry_msgs::PoseStamped start;   ///< chỉ hợp lệ khi has_goal
+        robot_geometry_msgs::PoseStamped goal;    ///< chỉ hợp lệ khi has_goal
+
+        std::vector<robot_protocol_msgs::Node> nodes;
+        std::vector<robot_protocol_msgs::Edge> edges;
+
+        /**
+         * Action của chặng, đã sắp theo sequenceId.
+         *
+         * Mission layer KHÔNG diễn giải `actionType` và không lọc gì — đó là việc của navigation
+         * runtime (D8). Ở đây chúng chỉ đi qua nguyên vẹn, đúng thứ tự.
+         */
+        std::vector<Action> actions;
+
+        /**
+         * Profile navigation hiệu lực của chặng có goal: `"position"`, `"docking"`,
+         * `"go_straight"` hoặc `"rotate"`. Mission chỉ-action phải để rỗng. Adapter hiện tại
+         * luôn ghi `"position"` rõ ràng cho chặng nav thường để trace/runtime biết chính xác
+         * planner nào sẽ được chọn; bridge vẫn chấp nhận rỗng như `position` để tương thích output
+         * cũ.
+         *
+         * Là **chuỗi** chứ không phải enum, và mission layer **không diễn giải** nó — cùng quy tắc
+         * với @ref actions. Mission layer không được biết tới navigation nên không có kiểu nào để
+         * diễn tả "profile"; lớp nối phía navigation mới dịch chuỗi này sang khái niệm của mình.
+         */
+        std::string motion_hint;
+
+        /**
+         * Marker chọn cặp planner docking. Rỗng hoặc không có override cấu hình thì navigation
+         * dùng profile docking mặc định. Đây không phải goal_frame: marker là khoá cấu hình, còn
+         * goal_frame là frame TF được resolve khi chặng bắt đầu.
+         */
+        std::string marker;
+
+        /**
+         * Đích lấy từ TF frame này thay vì từ @ref goal. Rỗng = dùng @ref goal.
+         *
+         * Chặng dò-rồi-tiến-vào không biết đích lúc được sinh ra: bước dò phía trước tạo ra frame
+         * này, và navigation tra nó tại thời điểm chặng được nhận.
+         */
+        std::string goal_frame;
+
+        /// Quãng đường tương đối [m] so với pose hiện tại; dương = tiến, âm = lùi. NaN = không dùng.
+        double relative_distance = std::numeric_limits<double>::quiet_NaN();
+    };
+
+} // namespace mission_adapters
+
+#endif  // MISSION_ADAPTERS_TYPES_H_

package.xml

@@ -1,51 +1,41 @@
 <package>
   <name>mission_adapters</name>
-  <version>0.7.10</version>
+  <version>0.2.0</version>
   <description>
-      mission_adapters is the second generation of the transform library, which lets
+    Lớp mission độc lập ROS: nhận yêu cầu từ nguồn ngoài (goal đơn lẻ, VDA5050 Order), chuyển thành
-      the user keep track of multiple coordinate frames over time. mission_adapters
+    hàng đợi mission, và giữ trạng thái của hàng đợi đó qua một event bus có thread riêng.
-      maintains the relationship between coordinate frames in a tree
-      structure buffered in time, and lets the user transform points,
-      vectors, etc between any two coordinate frames at any desired
-      point in time.
-  </description>
-  <author>Tully Foote</author>
-  <author>Eitan Marder-Eppstein</author>
-  <author>Wim Meeussen</author>
-  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
-  <license>BSD</license>
 
-  <url type="website">http://www.ros.org/wiki/mission_adapters</url>
+    Gói không biết navigation runtime nào đang chạy phía sau — mission được đẩy xuống qua cổng
+    NavigationClient, kết quả quay về theo MissionId. Nhờ vậy cùng một lớp mission dùng được cho
+    nhiều runtime khác nhau và test được mà không cần robot.
+
+    Nguồn mission là plugin nạp bằng Boost.DLL theo khoá library_path trong YAML, giống phần còn lại
+    của workspace; thêm một loại nguồn mới không phải sửa core.
+  </description>
+  <author>DuongTD</author>
+  <maintainer email="xroboticdevs@gmail.com">DuongTD</maintainer>
+  <license>BSD</license>
 
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>libconsole-bridge-dev</build_depend>
-  <run_depend>libconsole-bridge-dev</run_depend>
-
-  <build_depend>robot_costmap_2d</build_depend>
-  <build_depend>robot_nav_core</build_depend>
-  <build_depend>robot_nav_core2</build_depend>
-  <build_depend>robot_nav_msgs</build_depend>
-  <build_depend>robot_std_msgs</build_depend>
-  <build_depend>robot_geometry_msgs</build_depend>
   <build_depend>robot_cpp</build_depend>
-  <build_depend>tf3</build_depend>
-  <build_depend>robot_tf3_geometry_msgs</build_depend>
-  <build_depend>robot_visualization_msgs</build_depend>
-  <build_depend>robot_nav_2d_utils</build_depend>
-  <build_depend>data_convert</build_depend>
-
-  <run_depend>robot_costmap_2d</run_depend>
-  <run_depend>robot_nav_core</run_depend>
-  <run_depend>robot_nav_core2</run_depend>
-  <run_depend>robot_nav_msgs</run_depend>
-  <run_depend>robot_std_msgs</run_depend>
-  <run_depend>robot_geometry_msgs</run_depend>
   <run_depend>robot_cpp</run_depend>
-  <run_depend>tf3</run_depend>
+
-  <run_depend>robot_tf3_geometry_msgs</run_depend>
+  <build_depend>robot_time</build_depend>
-  <run_depend>robot_visualization_msgs</run_depend>
+  <run_depend>robot_time</run_depend>
-  <run_depend>robot_nav_2d_utils</run_depend>
+
-  <run_depend>data_convert</run_depend>
+  <build_depend>robot_geometry_msgs</build_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+
+  <!-- P2: header include thẳng robot_protocol_msgs/Order.h — trước đây sống nhờ transitive include. -->
+  <build_depend>robot_protocol_msgs</build_depend>
+  <run_depend>robot_protocol_msgs</run_depend>
+
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+
+  <build_depend>yaml-cpp</build_depend>
+  <run_depend>yaml-cpp</run_depend>
+
   <test_depend>gtest</test_depend>
 </package>

plugins/goal_source_adapter.cpp

@@ -0,0 +1,91 @@
+#include "goal_source_adapter.h"
+
+#include <cmath>
+
+#include <boost/dll/alias.hpp>
+
+namespace mission_plugins
+{
+
+mission_adapters::MissionSourceAdapter::Ptr GoalSourceAdapter::create()
+{
+    return std::make_shared<GoalSourceAdapter>();
+}
+
+bool GoalSourceAdapter::configure(const std::string& name, robot::NodeHandle& nh)
+{
+    (void)nh;   // nguồn này chưa có param riêng
+    name_ = name;
+    return true;
+}
+
+std::string GoalSourceAdapter::schema() const
+{
+    return mission_adapters::schema::kPoseStamped;
+}
+
+bool GoalSourceAdapter::validate(const mission_adapters::MissionRequest& request,
+                                 std::string& reason) const
+{
+    if (!request.pose)
+    {
+        reason = "request is missing the pose payload";
+        return false;
+    }
+
+    const auto& position = request.pose->pose.position;
+    const auto& orientation = request.pose->pose.orientation;
+
+    // NaN/Inf từ host phải bị chặn ngay tại biên: lọt xuống dưới thì mọi phép so khoảng cách tới
+    // goal đều trả false và robot chạy tới khi có người bấm dừng.
+    if (!std::isfinite(position.x) || !std::isfinite(position.y) || !std::isfinite(position.z))
+    {
+        reason = "goal contains NaN/Inf in position";
+        return false;
+    }
+
+    if (!std::isfinite(orientation.x) || !std::isfinite(orientation.y) ||
+        !std::isfinite(orientation.z) || !std::isfinite(orientation.w))
+    {
+        reason = "goal contains NaN/Inf in orientation";
+        return false;
+    }
+
+    // Quaternion toàn 0 là lỗi hay gặp khi host quên set orientation — nó không phải "hướng bất kỳ",
+    // nó là dữ liệu hỏng.
+    const double norm_squared = orientation.x * orientation.x +
+                                orientation.y * orientation.y +
+                                orientation.z * orientation.z +
+                                orientation.w * orientation.w;
+
+    if (std::sqrt(norm_squared) < kMinQuaternionNorm)
+    {
+        reason = "goal has a quaternion that cannot be normalized (norm ~ 0)";
+        return false;
+    }
+
+    return true;
+}
+
+mission_adapters::ConversionResult
+GoalSourceAdapter::convert(const mission_adapters::MissionRequest& request)
+{
+    mission_adapters::ConversionResult result;
+
+    if (!request.pose)
+        return result;
+
+    auto mission = std::make_shared<mission_adapters::Mission>();
+    mission->type = mission_adapters::MissionType::SIMPLE_GOAL;
+    mission->goal = *request.pose;
+    mission->motion_hint = "position";
+
+    // Goal đơn lẻ luôn thay việc đang chạy: người dùng bấm một đích mới nghĩa là bỏ đích cũ.
+    result.mode = mission_adapters::SubmitMode::kReplace;
+    result.missions.push_back(mission);
+    return result;
+}
+
+}  // namespace mission_plugins
+
+BOOST_DLL_ALIAS(mission_plugins::GoalSourceAdapter::create, GoalSourceAdapter)

plugins/goal_source_adapter.h

@@ -0,0 +1,55 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Nguồn mission từ một goal đơn lẻ (schema "geometry.pose_stamped").
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_PLUGINS_GOAL_SOURCE_ADAPTER_H_
+#define MISSION_ADAPTERS_PLUGINS_GOAL_SOURCE_ADAPTER_H_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <mission_adapters/adapter.h>
+
+namespace mission_plugins
+{
+
+/**
+ * @class GoalSourceAdapter
+ * @brief Goal đơn lẻ từ host -> đúng một mission.
+ *
+ * Không có state giữa các lần gọi, nhưng vẫn không dùng biến static: một tiến trình có thể chạy
+ * nhiều instance (nhiều robot mô phỏng) và state static sẽ nối chúng lại với nhau.
+ */
+class GoalSourceAdapter : public mission_adapters::MissionSourceAdapter
+{
+public:
+    /// @brief Factory được PluginRegistry nạp qua boost::dll::import_alias.
+    static mission_adapters::MissionSourceAdapter::Ptr create();
+
+    bool configure(const std::string& name, robot::NodeHandle& nh) override;
+
+    std::string schema() const override;
+
+    bool validate(const mission_adapters::MissionRequest& request,
+                  std::string& reason) const override;
+
+    mission_adapters::ConversionResult
+    convert(const mission_adapters::MissionRequest& request) override;
+
+private:
+    /// Dưới ngưỡng này thì quaternion coi như không mang hướng nào. [không đơn vị]
+    static constexpr double kMinQuaternionNorm = 1e-6;
+
+    std::string name_;
+};
+
+}  // namespace mission_plugins
+
+#endif  // MISSION_ADAPTERS_PLUGINS_GOAL_SOURCE_ADAPTER_H_

plugins/vda5050_source_adapter.cpp

@@ -0,0 +1,577 @@
+#include "vda5050_source_adapter.h"
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+#include <utility>
+
+#include <yaml-cpp/yaml.h>
+
+#include <boost/dll/alias.hpp>
+
+#include <robot/robot.h>
+
+namespace mission_plugins
+{
+namespace
+{
+bool isNavigationProfile(const std::string& profile)
+{
+    return profile.empty() || profile == "position" || profile == "docking" ||
+           profile == "go_straight" || profile == "rotate";
+}
+
+    using mission_adapters::Action;
+    using mission_adapters::ActionType;
+    using mission_adapters::ConversionResult;
+    using mission_adapters::Mission;
+    using mission_adapters::MissionType;
+    using mission_adapters::SubmitMode;
+
+    /// Gom action của edge và của node cuối vào mission, sắp theo sequenceId của VDA5050.
+    void collectActions(const std::shared_ptr<Mission>& mission)
+    {
+        for (const auto& edge : mission->edges)
+        {
+            for (const auto& action : edge.actions)
+            {
+                Action ma;
+                ma.sequenceId = edge.sequenceId;
+                ma.type       = ActionType::EDGE_ACTION;
+                ma.action     = action;
+                mission->actions.push_back(std::move(ma));
+            }
+        }
+
+        if (!mission->nodes.empty())
+        {
+            const auto& node = mission->nodes.back();
+            for (const auto& action : node.actions)
+            {
+                Action ma;
+                ma.sequenceId = node.sequenceId;
+                ma.type       = ActionType::NODE_ACTION;
+                ma.action     = action;
+                mission->actions.push_back(std::move(ma));
+            }
+        }
+
+        // stable_sort chứ không sort: `sequenceId` ở đây là của NODE/EDGE sở hữu action, nên mọi
+        // action trên cùng một node có khoá BẰNG NHAU. Với khoá bằng nhau, std::sort không bảo đảm
+        // giữ thứ tự — mà thứ tự đó chính là thứ tự trong mảng JSON, thứ VDA5050 quy định là thứ tự
+        // thực hiện. Bản cũ chạy đúng chỉ nhờ libstdc++ dùng insertion sort cho dải nhỏ.
+        std::stable_sort(mission->actions.begin(), mission->actions.end(),
+            [](const Action& a, const Action& b) {
+                return a.sequenceId < b.sequenceId;
+            });
+    }
+
+}  // namespace
+
+mission_adapters::MissionSourceAdapter::Ptr VDA5050SourceAdapter::create()
+{
+    return std::make_shared<VDA5050SourceAdapter>();
+}
+
+bool VDA5050SourceAdapter::configure(const std::string& name, robot::NodeHandle& nh)
+{
+    name_ = name;
+
+    nh.getParam(name + "/global_frame", global_frame_, std::string("map"));
+    if (global_frame_.empty())
+    {
+        robot::log_error("VDA5050SourceAdapter[%s]: global_frame is empty", name.c_str());
+        return false;
+    }
+
+    if (!loadCompoundActions(name, nh))
+        return false;
+
+    last_order_id_.clear();
+    last_order_update_id_ = 0;
+    converted_node_count_ = 0;
+    return true;
+}
+
+bool VDA5050SourceAdapter::loadCompoundActions(const std::string& name, robot::NodeHandle& nh)
+{
+    compound_actions_.clear();
+
+    YAML::Node table;
+    if (!nh.getParam(name + "/compound_actions", table) || !table.IsMap())
+        return true;   // Không khai bảng là hợp lệ: adapter chạy y như trước.
+
+    for (auto entry = table.begin(); entry != table.end(); ++entry)
+    {
+        const std::string action_type = entry->first.as<std::string>();
+        const YAML::Node& steps_node = entry->second["steps"];
+
+        if (action_type.empty() || !steps_node || !steps_node.IsSequence() || steps_node.size() == 0)
+        {
+            robot::log_error("VDA5050SourceAdapter[%s]: compound action '%s' has no 'steps' list",
+                             name.c_str(), action_type.c_str());
+            return false;
+        }
+
+        Steps steps;
+        for (std::size_t i = 0; i < steps_node.size(); ++i)
+        {
+            const YAML::Node& n = steps_node[i];
+            Step step;
+
+            try
+            {
+                if (n["action"])         step.action        = n["action"].as<std::string>();
+                if (n["move_to"])        step.move_to       = n["move_to"].as<std::string>();
+                if (n["move_to_param"])  step.move_to_param = n["move_to_param"].as<std::string>();
+                if (n["move"])           step.move          = n["move"].as<double>();
+                if (n["profile"])        step.motion_hint   = n["profile"].as<std::string>();
+                if (n["marker"])         step.marker        = n["marker"].as<std::string>();
+            }
+            catch (const YAML::Exception& ex)
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu is malformed: %s",
+                                 name.c_str(), action_type.c_str(), i, ex.what());
+                return false;
+            }
+
+            const int keys = (step.action.empty() ? 0 : 1) + (step.move_to.empty() ? 0 : 1) +
+                             (step.move_to_param.empty() ? 0 : 1) +
+                             (std::isfinite(step.move) ? 1 : 0);
+            if (keys != 1)
+            {
+                // Không đúng một từ khoá thì không có cách diễn giải nào là hiển nhiên đúng. Chặn ở
+                // boot thay vì đoán lúc order đầu tiên tới.
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu must have exactly one of "
+                                 "action / move_to / move_to_param / move (found %d)",
+                                 name.c_str(), action_type.c_str(), i, keys);
+                return false;
+            }
+
+            if (!isNavigationProfile(step.motion_hint))
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu has invalid profile '%s'",
+                                 name.c_str(), action_type.c_str(), i, step.motion_hint.c_str());
+                return false;
+            }
+            if (!step.action.empty() && !step.motion_hint.empty())
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu is action-only and must "
+                                 "not set profile",
+                                 name.c_str(), action_type.c_str(), i);
+                return false;
+            }
+            if (!step.marker.empty() && step.motion_hint != "docking")
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu sets marker but is not "
+                                 "a docking navigation step",
+                                 name.c_str(), action_type.c_str(), i);
+                return false;
+            }
+
+            steps.push_back(step);
+        }
+
+        compound_actions_[action_type] = std::move(steps);
+    }
+
+    // Chống đệ quy: một step sinh ra actionType mà chính nó cũng là compound thì expander sẽ mở rộng
+    // output của mình — vòng lặp vô hạn lúc convert.
+    for (const auto& entry : compound_actions_)
+    {
+        for (const Step& step : entry.second)
+        {
+            if (!step.action.empty() && compound_actions_.count(step.action) != 0)
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: compound '%s' emits '%s' which is itself "
+                                 "a compound action — that would recurse",
+                                 name.c_str(), entry.first.c_str(), step.action.c_str());
+                return false;
+            }
+        }
+    }
+
+    robot::log_info("VDA5050SourceAdapter[%s]: %zu compound action(s) loaded", name.c_str(),
+                    compound_actions_.size());
+    return true;
+}
+
+const VDA5050SourceAdapter::Steps*
+VDA5050SourceAdapter::findCompound(const std::string& action_type) const
+{
+    const auto it = compound_actions_.find(action_type);
+    return it == compound_actions_.end() ? nullptr : &it->second;
+}
+
+std::string VDA5050SourceAdapter::schema() const
+{
+    return mission_adapters::schema::kVda5050Order;
+}
+
+size_t VDA5050SourceAdapter::countReleasedNodes(const robot_protocol_msgs::Order& order)
+{
+    // VDA5050: base là tiền tố của danh sách node — dừng ở node đầu tiên chưa release.
+    size_t count = 0;
+    while (count < order.nodes.size() && order.nodes[count].released)
+        ++count;
+
+    return count;
+}
+
+size_t VDA5050SourceAdapter::countReleasedEdges(const robot_protocol_msgs::Order& order,
+                                                size_t released_node_count)
+{
+    if (released_node_count < 2)
+        return 0;
+
+    size_t count = 0;
+    const size_t limit = std::min(order.edges.size(), released_node_count - 1);
+    while (count < limit && order.edges[count].released)
+        ++count;
+
+    return count;
+}
+
+size_t VDA5050SourceAdapter::executableNodeCount(const robot_protocol_msgs::Order& order) const
+{
+    const size_t released = countReleasedNodes(order);
+    if (released > 0)
+        return released;
+
+    // Không node nào released. Theo VDA5050 order phải có ít nhất một base node, nên trường hợp này
+    // gần như luôn là host không điền `released`. Coi cả order là base — im lặng không chạy gì sẽ
+    // khiến fleet manager chờ vô hạn mà không có dấu hiệu nào.
+    robot::log_warning("VDA5050SourceAdapter[%s]: order '%s' has no released node — treating the "
+                       "whole order as base (did the host leave the 'released' field out?)",
+                       name_.c_str(), order.orderId.c_str());
+    return order.nodes.size();
+}
+
+robot_geometry_msgs::PoseStamped
+VDA5050SourceAdapter::toPose(const robot_protocol_msgs::Node& node) const
+{
+    robot_geometry_msgs::PoseStamped pose;
+    pose.header.frame_id = global_frame_;
+    pose.pose.position.x = node.nodePosition.x;   // [m]
+    pose.pose.position.y = node.nodePosition.y;   // [m]
+    pose.pose.position.z = 0.0;
+
+    // theta [rad] quanh trục z -> quaternion.
+    const double half_theta = 0.5 * node.nodePosition.theta;
+    pose.pose.orientation.x = 0.0;
+    pose.pose.orientation.y = 0.0;
+    pose.pose.orientation.z = std::sin(half_theta);
+    pose.pose.orientation.w = std::cos(half_theta);
+
+    return pose;
+}
+
+bool VDA5050SourceAdapter::validate(const mission_adapters::MissionRequest& request,
+                                    std::string& reason) const
+{
+    if (!request.order)
+    {
+        reason = "request is missing the order payload";
+        return false;
+    }
+
+    const auto& order = *request.order;
+
+    if (order.nodes.empty())
+    {
+        reason = "order has no node";
+        return false;
+    }
+
+    // VDA5050: n node liên thông cần đúng n-1 edge. Thiếu edge nghĩa là đồ thị đứt đoạn, cắt chặng
+    // theo chỉ số sẽ lấy nhầm edge của đoạn khác.
+    if (order.nodes.size() > 1 && order.edges.size() < order.nodes.size() - 1)
+    {
+        reason = "fewer edges than nodes - 1";
+        return false;
+    }
+
+    for (const auto& node : order.nodes)
+    {
+        if (!std::isfinite(node.nodePosition.x) || !std::isfinite(node.nodePosition.y) ||
+            !std::isfinite(node.nodePosition.theta))
+        {
+            reason = "nodePosition contains NaN/Inf at node '" + node.nodeId + "'";
+            return false;
+        }
+    }
+
+    // Bản cập nhật cũ hơn (hoặc phát lại) của order đang chạy: từ chối thay vì chạy lại tuyến đường.
+    if (!order.orderId.empty() && order.orderId == last_order_id_ &&
+        order.orderUpdateId <= last_order_update_id_)
+    {
+        reason = "orderUpdateId " + std::to_string(order.orderUpdateId) +
+                 " is not newer than the running one (" + std::to_string(last_order_update_id_) + ")";
+        return false;
+    }
+
+    return true;
+}
+
+
+bool VDA5050SourceAdapter::expandCompound(
+    const std::shared_ptr<mission_adapters::Mission>& leg,
+    std::vector<std::shared_ptr<mission_adapters::Mission>>& out) const
+{
+    if (compound_actions_.empty() || leg->actions.empty())
+        return true;   // Không có gì để mở rộng — đường đi thường.
+
+    std::vector<Action> buffer;      // action thường đang chờ xả
+    std::vector<Action> on_the_leg;  // bộ đệm ĐẦU TIÊN: ở lại trên chặng nav
+    bool first_flush = true;
+    bool expanded_any = false;
+
+    // Chặng chỉ-action sinh ra từ bộ đệm sau lần xả đầu.
+    auto flush = [&](void) {
+        if (buffer.empty())
+            return;
+        if (first_flush)
+        {
+            on_the_leg = buffer;
+        }
+        else
+        {
+            auto extra = std::make_shared<Mission>();
+            extra->type = MissionType::VDA5050_ORDER;
+            extra->has_goal = false;
+            extra->actions = buffer;
+            out.push_back(std::move(extra));
+        }
+        buffer.clear();
+    };
+
+    for (const Action& entry : leg->actions)
+    {
+        // Compound chỉ áp cho NODE_ACTION: một chuỗi dò-rồi-tiến-vào không có nghĩa khi gắn vào một
+        // cạnh mà robot đang đi trên đó.
+        const Steps* steps = (entry.type == ActionType::NODE_ACTION)
+                                 ? findCompound(entry.action.actionType)
+                                 : nullptr;
+
+        if (steps == nullptr)
+        {
+            if (entry.type == ActionType::EDGE_ACTION &&
+                findCompound(entry.action.actionType) != nullptr)
+            {
+                robot::log_warning("VDA5050SourceAdapter[%s]: '%s' is a compound action but sits on "
+                                   "an EDGE — running it as a plain action",
+                                   name_.c_str(), entry.action.actionType.c_str());
+            }
+            buffer.push_back(entry);
+            continue;
+        }
+
+        flush();
+        first_flush = false;
+        expanded_any = true;
+
+        for (const Step& step : *steps)
+        {
+            auto sub = std::make_shared<Mission>();
+            sub->type = MissionType::VDA5050_ORDER;
+
+            if (!step.action.empty())
+            {
+                // Action nội bộ: mang NGUYÊN actionParameters của action gốc — handler cần biết dò
+                // trạm nào, và nó là chỗ duy nhất hiểu ý nghĩa các tham số đó.
+                Action generated;
+                generated.type = ActionType::NODE_ACTION;
+                generated.sequenceId = entry.sequenceId;
+                generated.action = entry.action;
+                generated.action.actionType = step.action;
+                generated.action.actionId = entry.action.actionId + "-" + step.action;
+
+                sub->has_goal = false;
+                sub->actions.push_back(std::move(generated));
+            }
+            else
+            {
+                sub->has_goal = true;
+                // Mission nav luôn tự mô tả profile hiệu lực. Action-only giữ chuỗi rỗng để không
+                // bao giờ bị hiểu nhầm là một yêu cầu điều khiển.
+                sub->motion_hint = step.motion_hint.empty() ? "position" : step.motion_hint;
+                sub->marker = step.marker;
+                sub->start = leg->start;
+                sub->goal = leg->goal;   // chỗ dựa; đích thật đến muộn qua goal_frame/relative_distance
+
+                if (!step.move_to.empty())
+                {
+                    sub->goal_frame = step.move_to;
+                }
+                else if (!step.move_to_param.empty())
+                {
+                    // Thay thế xảy ra tại CONVERT: runtime không bao giờ thấy placeholder, và thiếu
+                    // tham số thì order bị từ chối trước khi robot nhúc nhích.
+                    const auto it = std::find_if(
+                        entry.action.actionParameters.begin(), entry.action.actionParameters.end(),
+                        [&step](const robot_protocol_msgs::ActionParameter& p) {
+                            return p.key == step.move_to_param;
+                        });
+
+                    if (it == entry.action.actionParameters.end() || it->value.empty())
+                    {
+                        robot::log_error("VDA5050SourceAdapter[%s]: compound action '%s' (actionId "
+                                         "'%s') requires parameter '%s' but it is missing",
+                                         name_.c_str(), entry.action.actionType.c_str(),
+                                         entry.action.actionId.c_str(), step.move_to_param.c_str());
+                        return false;
+                    }
+                    sub->goal_frame = it->value;
+                }
+                else
+                {
+                    sub->relative_distance = step.move;
+                }
+            }
+
+            out.push_back(std::move(sub));
+        }
+    }
+
+    flush();
+
+    if (expanded_any)
+        leg->actions = on_the_leg;
+
+    return true;
+}
+
+ConversionResult VDA5050SourceAdapter::convert(const mission_adapters::MissionRequest& request)
+{
+    ConversionResult result;
+
+    std::string reason;
+    if (!validate(request, reason))
+    {
+        robot::log_warning("VDA5050SourceAdapter[%s]: dropping the order — %s", name_.c_str(),
+                           reason.c_str());
+        return result;
+    }
+
+    const auto& order = *request.order;
+
+    const size_t node_count = executableNodeCount(order);
+    const size_t edge_count = (countReleasedNodes(order) > 0)
+                                  ? countReleasedEdges(order, node_count)
+                                  : order.edges.size();
+
+    // Order update của đúng order đang chạy: chỉ sinh phần vừa được release thêm.
+    const bool is_update = !order.orderId.empty() && order.orderId == last_order_id_;
+
+    size_t start_node_idx = 0;
+    if (is_update)
+    {
+        if (converted_node_count_ >= node_count)
+        {
+            robot::log_info("VDA5050SourceAdapter[%s]: order '%s' update %u released no further "
+                            "node — there is no new work",
+                            name_.c_str(), order.orderId.c_str(),
+                            static_cast<unsigned>(order.orderUpdateId));
+
+            last_order_update_id_ = order.orderUpdateId;
+            return result;
+        }
+
+        // Node cuối đã chuyển đổi trở thành node xuất phát của chặng tiếp theo.
+        start_node_idx = converted_node_count_ - 1;
+        result.mode = SubmitMode::kAppend;
+    }
+
+    // Chặng được cắt tại mỗi node có action: robot chạy tới node đó rồi mới thực hiện action.
+    std::vector<size_t> action_node_indices;
+    for (size_t i = start_node_idx + 1; i < node_count; ++i)
+    {
+        if (!order.nodes[i].actions.empty())
+            action_node_indices.push_back(i);
+    }
+
+    // Node xuất phát có action (và chưa từng được chuyển đổi) cũng là một chặng — chặng chỉ-action.
+    if (!is_update && !order.nodes[start_node_idx].actions.empty())
+        action_node_indices.insert(action_node_indices.begin(), start_node_idx);
+
+    auto makeMission = [&](size_t from, size_t to) {
+        auto mission = std::make_shared<Mission>();
+        mission->type = MissionType::VDA5050_ORDER;
+
+        mission->nodes.assign(order.nodes.begin() + from, order.nodes.begin() + to + 1);
+
+        if (to > from)
+        {
+            const size_t edge_to = std::min(to, edge_count);
+            if (from < edge_to)
+                mission->edges.assign(order.edges.begin() + from, order.edges.begin() + edge_to);
+        }
+
+        // D8: chặng một node là chặng chỉ-có-action (action nằm ngay tại node xuất phát) — không có
+        // quãng đường nào để đi. Quy tắc thuần cấu trúc, adapter không cần biết robot đang ở đâu.
+        mission->has_goal = (to > from);
+
+        if (mission->has_goal)
+        {
+            // Mission self-contained: consumer không phải suy goal ra từ nodes (A4).
+            mission->start = toPose(order.nodes[from]);
+            mission->goal  = toPose(order.nodes[to]);
+            mission->motion_hint = "position";
+        }
+
+        collectActions(mission);
+        return mission;
+    };
+
+    // Mission không goal mà cũng không action là mission rỗng — không sinh ra nó.
+    auto isMeaningful = [](const std::shared_ptr<Mission>& mission) {
+        return mission->has_goal || !mission->actions.empty();
+    };
+
+    size_t segment_start = start_node_idx;
+
+    for (const size_t action_node_idx : action_node_indices)
+    {
+        auto mission = makeMission(segment_start, action_node_idx);
+
+        // Mở rộng TRƯỚC isMeaningful: hàm này gỡ action compound khỏi chặng, và một chặng chỉ-action
+        // sau khi gỡ có thể trở thành rỗng.
+        std::vector<std::shared_ptr<Mission>> expanded;
+        if (!expandCompound(mission, expanded))
+        {
+            // Thiếu tham số cấu trúc: bỏ CẢ order. Một order chạy nửa vời — robot tới trạm sạc rồi
+            // không sạc — nguy hiểm hơn là không chạy.
+            result.missions.clear();
+            return result;
+        }
+
+        if (isMeaningful(mission))
+            result.missions.push_back(std::move(mission));
+
+        for (auto& sub : expanded)
+            result.missions.push_back(std::move(sub));
+
+        segment_start = action_node_idx;
+    }
+
+    // Đoạn còn lại sau node có action cuối cùng.
+    if (segment_start + 1 < node_count)
+    {
+        auto mission = makeMission(segment_start, node_count - 1);
+        if (isMeaningful(mission))
+            result.missions.push_back(std::move(mission));
+    }
+
+    if (result.missions.empty())
+        return result;
+
+    last_order_id_        = order.orderId;
+    last_order_update_id_ = order.orderUpdateId;
+    converted_node_count_ = node_count;
+
+    return result;
+}
+
+}  // namespace mission_plugins
+
+BOOST_DLL_ALIAS(mission_plugins::VDA5050SourceAdapter::create, VDA5050SourceAdapter)

plugins/vda5050_source_adapter.h

@@ -0,0 +1,146 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Nguồn mission từ VDA5050 Order (schema "vda5050.order").
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_PLUGINS_VDA5050_SOURCE_ADAPTER_H_
+#define MISSION_ADAPTERS_PLUGINS_VDA5050_SOURCE_ADAPTER_H_
+
+#include <cstddef>
+#include <limits>
+#include <cstdint>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <mission_adapters/adapter.h>
+
+namespace mission_plugins
+{
+
+/**
+ * @class VDA5050SourceAdapter
+ * @brief Cắt một VDA5050 Order thành các chặng, mỗi chặng kết thúc tại một node có action.
+ *
+ * Ba điểm conformance mà adapter này chịu trách nhiệm:
+ *
+ * 1. **base / horizon** — chỉ phần `released == true` được thực thi. Horizon là dự định của fleet
+ *    manager, chưa được phép chạy; robot đi vào đó là đi vào đoạn đường chưa ai cho phép.
+ * 2. **orderId / orderUpdateId** — phân biệt yêu cầu MỚI (thay hàng đợi) với bản CẬP NHẬT của yêu
+ *    cầu đang chạy (nối tiếp). Nhầm hai thứ này thì mỗi lần fleet manager release thêm horizon,
+ *    robot lại huỷ và chạy lại chặng đang đi.
+ * 3. **goal / start self-contained** — mission mang sẵn pose đích, consumer không phải tự đoán từ
+ *    `nodes.back()`.
+ *
+ * State là member, không phải static local: nhiều instance trong cùng tiến trình không được nhìn
+ * thấy order của nhau.
+ */
+class VDA5050SourceAdapter : public mission_adapters::MissionSourceAdapter
+{
+public:
+    /// @brief Factory được PluginRegistry nạp qua boost::dll::import_alias.
+    static mission_adapters::MissionSourceAdapter::Ptr create();
+
+    /**
+     * @brief Đọc param riêng của instance.
+     *
+     * Param (namespace `<name>`):
+     *  - `global_frame` [string, mặc định "map"]: frame gán cho pose sinh ra từ nodePosition.
+     *  - `compound_actions` [map, optional]: action "phải dò rồi mới biết đích" — xem @ref Step.
+     */
+    bool configure(const std::string& name, robot::NodeHandle& nh) override;
+
+    std::string schema() const override;
+
+    bool validate(const mission_adapters::MissionRequest& request,
+                  std::string& reason) const override;
+
+    mission_adapters::ConversionResult
+    convert(const mission_adapters::MissionRequest& request) override;
+
+private:
+    /**
+     * @struct Step
+     * @brief Một bước trong chuỗi mở rộng của compound action.
+     *
+     * Engine hiểu đúng bốn từ khoá và **không biết** `charge`, `dock_target` hay `LiftFork` nghĩa là
+     * gì — đó là điều kiện để cùng một engine dùng lại cho model robot khác: thêm model = thêm YAML
+     * + một `.so` handler, không sửa dòng nào ở đây.
+     *
+     * Mỗi step phải có ĐÚNG MỘT trong bốn khoá đầu; sai cú pháp thì `configure()` từ chối lúc **boot**.
+     */
+    struct Step
+    {
+        std::string action;          ///< `action: <type>`      -> chặng chỉ-action
+        std::string move_to;         ///< `move_to: <frame>`    -> chặng nav, frame cố định
+        std::string move_to_param;   ///< `move_to_param: <key>`-> frame lấy từ actionParameters
+        double      move = std::numeric_limits<double>::quiet_NaN();  ///< `move: <m>` tương đối
+
+        std::string motion_hint;     ///< `profile:` -> position | docking | go_straight | rotate
+        std::string marker;          ///< `marker:` -> khoá override planner, chỉ hợp lệ với docking
+    };
+
+    /// @brief Chuỗi chặng thay cho một action. Bảng chỉ giữ CẤU TRÚC, dữ liệu tới từ order.
+    using Steps = std::vector<Step>;
+
+    /// @brief Nạp bảng `compound_actions`. Trả false nếu bảng có mà khai sai — lỗi nổ lúc boot.
+    bool loadCompoundActions(const std::string& name, robot::NodeHandle& nh);
+
+    /// @brief Tra bảng theo actionType. nullptr nếu là action thường.
+    const Steps* findCompound(const std::string& action_type) const;
+
+    /**
+     * @brief Mở rộng action của một chặng thành chuỗi chặng, giữ nguyên thứ tự mảng JSON.
+     * @param leg Chặng nav tới node; action compound sẽ được GỠ khỏi nó.
+     * @param out Nơi nối thêm chặng sinh ra.
+     * @return false nếu thiếu tham số cấu trúc — bên gọi phải bỏ CẢ order.
+     *
+     * Action thường tích vào bộ đệm; gặp compound thì xả bộ đệm rồi phát chuỗi. Bộ đệm ĐẦU TIÊN nằm
+     * lại trên chặng nav, các bộ đệm sau thành chặng chỉ-action. Nhờ vậy `[MutedOn, charge, MutedOff]`
+     * cho ra `MutedOff` **sau** chuỗi charge — gom hết vào chặng nav sẽ bật lại cảm biến an toàn
+     * trước khi robot lùi vào trạm.
+     */
+    bool expandCompound(const std::shared_ptr<mission_adapters::Mission>& leg,
+                        std::vector<std::shared_ptr<mission_adapters::Mission>>& out) const;
+
+    /// Số node đầu tiên có released == true. 0 nghĩa là không node nào được release.
+    static size_t countReleasedNodes(const robot_protocol_msgs::Order& order);
+
+    /// Số edge thuộc phần base, không vượt quá `released_node_count - 1`.
+    static size_t countReleasedEdges(const robot_protocol_msgs::Order& order,
+                                     size_t released_node_count);
+
+    /**
+     * @brief Phần order được phép thực thi.
+     * @return số node base; 0 nếu order không dùng trường `released` (xem ghi chú trong .cpp).
+     */
+    size_t executableNodeCount(const robot_protocol_msgs::Order& order) const;
+
+    /// Dựng PoseStamped từ nodePosition (theta [rad] -> quaternion quanh trục z).
+    robot_geometry_msgs::PoseStamped toPose(const robot_protocol_msgs::Node& node) const;
+
+    std::string name_;
+
+    /// Frame gán cho goal/start. VDA5050 `mapId` là danh tính bản đồ, KHÔNG phải frame TF.
+    std::string global_frame_ = "map";
+
+    /// actionType -> chuỗi chặng. Rỗng = không action nào cần mở rộng, adapter chạy như trước.
+    std::map<std::string, Steps> compound_actions_;
+
+    /// Order đang chạy — dùng để nhận ra order update so với order mới.
+    std::string last_order_id_;
+    std::uint32_t last_order_update_id_ = 0;
+
+    /// Số node base đã chuyển đổi của order đang chạy; điểm bắt đầu cho phần release thêm.
+    size_t converted_node_count_ = 0;
+};
+
+}  // namespace mission_plugins
+
+#endif  // MISSION_ADAPTERS_PLUGINS_VDA5050_SOURCE_ADAPTER_H_

src/event.cpp

@@ -0,0 +1,71 @@
+#include <mission_adapters/event.h>
+
+#include <utility>
+
+namespace mission_adapters
+{
+    // ─────────────────────────────────────────────────────────────────────────
+    // EventBus
+    // ─────────────────────────────────────────────────────────────────────────
+
+    void EventBus::push(Event event)
+    {
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            event.sequence = next_sequence_++;
+            queue_.push(std::move(event));
+        }
+        cv_.notify_one();
+    }
+
+    void EventBus::pushEmergency(Event event)
+    {
+        // Bật cờ TRƯỚC khi xếp hàng: bên tiêu thụ thấy được emergency ngay cả khi nó đang bận xử lý
+        // một sự kiện khác và chưa quay lại hàng đợi.
+        emergency_pending_.store(true, std::memory_order_release);
+        push(std::move(event));
+    }
+
+    bool EventBus::pop(Event& event)
+    {
+        std::unique_lock<std::mutex> lock(mutex_);
+        cv_.wait(lock, [this] { return stop_ || !queue_.empty(); });
+
+        if (stop_ || queue_.empty()) return false;
+
+        event = std::move(queue_.front());
+        queue_.pop();
+        return true;
+    }
+
+    bool EventBus::takeEmergency()
+    {
+        return emergency_pending_.exchange(false, std::memory_order_acq_rel);
+    }
+
+    bool EventBus::emergencyPending() const
+    {
+        return emergency_pending_.load(std::memory_order_acquire);
+    }
+
+    void EventBus::stop()
+    {
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            stop_ = true;
+        }
+        cv_.notify_all();
+    }
+
+    void EventBus::reset()
+    {
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            stop_ = false;
+            next_sequence_ = 0;
+            queue_ = {};
+        }
+        emergency_pending_.store(false, std::memory_order_release);
+    }
+
+} // namespace mission_adapters

src/event_processor.cpp

@@ -0,0 +1,214 @@
+#include <mission_adapters/event_processor.h>
+
+#include <utility>
+
+#include <robot/robot.h>
+
+namespace mission_adapters
+{
+    // ─────────────────────────────────────────────────────────────────────────
+    // EventProcessor
+    // ─────────────────────────────────────────────────────────────────────────
+
+    EventProcessor::EventProcessor(MissionManager& mission_manager, PluginRegistry& registry)
+        : mission_manager_(mission_manager)
+        , registry_(registry)
+    {}
+
+    EventProcessor::~EventProcessor() { stop(); }
+
+    void EventProcessor::start()
+    {
+        if (running_) return;
+        running_ = true;
+        event_bus_.reset();
+        worker_ = std::thread(&EventProcessor::spin, this);
+    }
+
+    void EventProcessor::stop()
+    {
+        running_ = false;
+        event_bus_.stop();
+        if (worker_.joinable()) worker_.join();
+    }
+
+    void EventProcessor::spin()
+    {
+        Event event;
+        while (running_)
+        {
+            // Emergency được xử lý trước mọi thứ khác, kể cả khi hàng đợi đang dài.
+            handlePendingEmergency();
+
+            if (!event_bus_.pop(event))
+                continue;
+
+            // Emergency có thể vừa tới trong lúc đang chờ pop(): xử lý nó trước sự kiện vừa lấy ra.
+            handlePendingEmergency();
+
+            process(event);
+        }
+    }
+
+    void EventProcessor::handlePendingEmergency()
+    {
+        if (event_bus_.takeEmergency())
+            mission_manager_.emergency();
+    }
+
+    void EventProcessor::handleRequest(const MissionRequest& request)
+    {
+        MissionSourceAdapter* adapter = registry_.find(request.schema);
+        if (!adapter)
+        {
+            robot::log_error("EventProcessor: no source declares schema '%s'",
+                             request.schema.c_str());
+            return;
+        }
+
+        std::string reason;
+        if (!adapter->validate(request, reason))
+        {
+            // A1: payload hỏng bị chặn tại đây, hàng đợi đang chạy không bị đụng tới.
+            robot::log_warning("EventProcessor: rejecting request schema '%s' — %s",
+                               request.schema.c_str(), reason.c_str());
+            return;
+        }
+
+        const ConversionResult result = adapter->convert(request);
+
+        // Bất biến D8, kiểm ở core chứ không chỉ trong adapter: một plugin bên thứ ba sinh ra
+        // mission không goal và cũng không action sẽ khiến navigation nhận một chặng không có việc
+        // gì để làm, và chặng đó không bao giờ báo kết quả về.
+        for (const auto& mission : result.missions)
+        {
+            if (!mission)
+            {
+                robot::log_error("EventProcessor: adapter for schema '%s' returned a null mission "
+                                 "— dropping the whole batch",
+                                 request.schema.c_str());
+                return;
+            }
+
+            if (!mission->has_goal && mission->actions.empty())
+            {
+                robot::log_error("EventProcessor: adapter for schema '%s' produced a mission with "
+                                 "neither goal nor action — dropping the whole batch",
+                                 request.schema.c_str());
+                return;
+            }
+        }
+
+        if (result.empty())
+        {
+            robot::log_warning("EventProcessor: request schema '%s' produced no mission — the "
+                               "queue is left untouched", request.schema.c_str());
+            return;
+        }
+
+        if (result.mode == SubmitMode::kAppend)
+            mission_manager_.append(result.missions);
+        else
+            mission_manager_.submit(result.missions);
+    }
+
+    void EventProcessor::process(const Event& event)
+    {
+        switch (event.type)
+        {
+            case EventType::SUBMIT_REQUEST:  handleRequest(event.request);                 break;
+            case EventType::NAV_DONE:        mission_manager_.onNavigationDone(event.mission_id);   break;
+            case EventType::NAV_FAILED:      mission_manager_.onNavigationFailed(event.mission_id); break;
+            case EventType::PAUSE:           mission_manager_.pause();                     break;
+            case EventType::RESUME:          mission_manager_.resume();                    break;
+            case EventType::CANCEL:          mission_manager_.cancel();                    break;
+
+            // Đã được xử lý ngay lúc phát qua cờ out-of-band; lần gọi này là idempotent và chỉ giữ
+            // cho dòng thời gian trong log liền mạch.
+            case EventType::EMERGENCY:       mission_manager_.emergency();                 break;
+
+            case EventType::CLEAR_EMERGENCY: mission_manager_.clearEmergency();            break;
+            default:
+                robot::log_error("EventProcessor: event of unknown type");
+                break;
+        }
+    }
+
+    void EventProcessor::submitRequest(const MissionRequest& request)
+    {
+        if (request.schema.empty())
+        {
+            robot::log_error("EventProcessor: request declares no schema");
+            return;
+        }
+
+        Event event;
+        event.type    = EventType::SUBMIT_REQUEST;
+        event.request = request;
+        event_bus_.push(std::move(event));
+    }
+
+    void EventProcessor::orderEvent(const robot_protocol_msgs::Order& order)
+    {
+        submitRequest(MissionRequest::fromOrder(order));
+    }
+
+    void EventProcessor::goalEvent(const robot_geometry_msgs::PoseStamped& goal)
+    {
+        submitRequest(MissionRequest::fromPose(goal));
+    }
+
+    void EventProcessor::pauseEvent()
+    {
+        Event event;
+        event.type = EventType::PAUSE;
+        event_bus_.push(std::move(event));
+    }
+
+    void EventProcessor::resumeEvent()
+    {
+        Event event;
+        event.type = EventType::RESUME;
+        event_bus_.push(std::move(event));
+    }
+
+    void EventProcessor::cancelEvent()
+    {
+        Event event;
+        event.type = EventType::CANCEL;
+        event_bus_.push(std::move(event));
+    }
+
+    void EventProcessor::navDoneEvent(MissionId id)
+    {
+        Event event;
+        event.type = EventType::NAV_DONE;
+        event.mission_id = id;
+        event_bus_.push(std::move(event));
+    }
+
+    void EventProcessor::navFailedEvent(MissionId id)
+    {
+        Event event;
+        event.type = EventType::NAV_FAILED;
+        event.mission_id = id;
+        event_bus_.push(std::move(event));
+    }
+
+    void EventProcessor::emergencyEvent()
+    {
+        Event event;
+        event.type = EventType::EMERGENCY;
+        event_bus_.pushEmergency(std::move(event));
+    }
+
+    void EventProcessor::clearEmergencyEvent()
+    {
+        // Không đi đường out-of-band: gỡ emergency là hành động có chủ đích của người vận hành,
+        // phải xếp sau mọi thứ đã phát trước nó.
+        Event event;
+        event.type = EventType::CLEAR_EMERGENCY;
+        event_bus_.push(std::move(event));
+    }
+
+} // namespace mission_adapters

src/mission_adapters.cpp

@@ -1,529 +0,0 @@
-//file mission_adapters.cpp
-#include <algorithm>
-#include <mission_adapters/mission_adapters.h>
-
-namespace mission_adapters
-{
-    // ─────────────────────────────────────────────────────────────────────────
-    // EventBus
-    // ─────────────────────────────────────────────────────────────────────────
-
-    void EventBus::push(const Event& event)
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        Event queued_event = event;
-        queued_event.sequence = next_sequence_++;
-        queue_.push(std::move(queued_event));
-        cv_.notify_one();
-    }
-
-    void EventBus::push(Event&& event)
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        event.sequence = next_sequence_++;
-        queue_.push(std::move(event));
-        cv_.notify_one();
-    }
-
-    bool EventBus::pop(Event& event)
-    {
-        std::unique_lock<std::mutex> lock(mutex_);
-        cv_.wait(lock, [this] { return stop_ || !queue_.empty(); });
-
-        if (stop_ || queue_.empty()) return false;
-
-        event = queue_.top();
-        queue_.pop();
-        return true;
-    }
-
-    void EventBus::stop()
-    {
-        {
-            std::lock_guard<std::mutex> lock(mutex_);
-            stop_ = true;
-        }
-        cv_.notify_all();
-    }
-
-    void EventBus::reset()
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        stop_ = false;
-        next_sequence_ = 0;
-        while (!queue_.empty()) queue_.pop();
-    }
-
-    // ─────────────────────────────────────────────────────────────────────────
-    // GoalAdapter
-    // ─────────────────────────────────────────────────────────────────────────
-
-    std::vector<std::shared_ptr<Mission>>
-    GoalAdapter::convert(const robot_geometry_msgs::PoseStamped& goal)
-    {
-        auto mission = std::make_shared<Mission>();
-        mission->type     = MissionType::SIMPLE_GOAL;
-        mission->priority = 0;
-        mission->goal     = goal;
-
-        return {mission};
-    }
-
-    // ─────────────────────────────────────────────────────────────────────────
-    // VDA5050Adapter
-    // ─────────────────────────────────────────────────────────────────────────
-
-    std::vector<std::shared_ptr<Mission>>
-    VDA5050Adapter::convert(const robot_protocol_msgs::Order& order)
-    {
-        std::vector<std::shared_ptr<Mission>> missions;
-        std::vector<size_t> action_node_indices;
-
-        if (order.nodes.empty()) return missions;
-
-        if (order.nodes.size() > 1 && order.edges.size() < order.nodes.size() - 1)
-        {
-            robot::log_error("Invalid VDA5050 order: edge count is smaller than node_count - 1");
-            return missions;
-        }
-
-        for (size_t i = 0; i < order.nodes.size(); ++i)
-        {
-            if (!order.nodes[i].actions.empty())
-                action_node_indices.push_back(i);
-        }
-
-        size_t start_node_idx = 0;
-
-        for (size_t i = 0; i < action_node_indices.size(); ++i)
-        {
-            size_t end_node_idx = action_node_indices[i];
-
-            auto mission = std::make_shared<Mission>();
-            mission->type     = MissionType::VDA5050_ORDER;
-            mission->priority = 0;
-
-            mission->nodes.assign(
-                order.nodes.begin() + start_node_idx,
-                order.nodes.begin() + end_node_idx + 1);
-
-            if (end_node_idx > start_node_idx)
-            {
-                mission->edges.assign(
-                    order.edges.begin() + start_node_idx,
-                    order.edges.begin() + end_node_idx);
-            }
-
-            missions.push_back(mission);
-            start_node_idx = end_node_idx;
-        }
-
-        // Remaining segment after the last action node
-        if (start_node_idx < order.nodes.size() - 1)
-        {
-            auto mission = std::make_shared<Mission>();
-            mission->type     = MissionType::VDA5050_ORDER;
-            mission->priority = 0;
-
-            mission->nodes.assign(
-                order.nodes.begin() + start_node_idx,
-                order.nodes.end());
-
-            if (start_node_idx < order.edges.size())
-            {
-                mission->edges.assign(
-                    order.edges.begin() + start_node_idx,
-                    order.edges.end());
-            }
-
-            missions.push_back(mission);
-        }
-
-        // Collect and sort actions
-        for (auto& mission : missions)
-        {
-            for (const auto& edge : mission->edges)
-            {
-                for (const auto& action : edge.actions)
-                {
-                    Action ma;
-                    ma.sequenceId = edge.sequenceId;
-                    ma.type       = ActionType::EDGE_ACTION;
-                    ma.action     = action;
-                    mission->actions.push_back(std::move(ma));
-                }
-            }
-
-            const auto& node = mission->nodes.back();
-            for (const auto& action : node.actions)
-            {
-                Action ma;
-                ma.sequenceId = node.sequenceId;
-                ma.type       = ActionType::NODE_ACTION;
-                ma.action     = action;
-                mission->actions.push_back(std::move(ma));
-            }
-
-            std::sort(mission->actions.begin(), mission->actions.end(),
-                [](const Action& a, const Action& b) {
-                    return a.sequenceId < b.sequenceId;
-                });
-        }
-
-        return missions;
-    }
-
-    // ─────────────────────────────────────────────────────────────────────────
-    // MissionManager
-    // ─────────────────────────────────────────────────────────────────────────
-
-    void MissionManager::submit(const std::vector<std::shared_ptr<Mission>>& missions)
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-
-        if (state_ == MissionState::EMERGENCY)
-            return;
-
-        if (!mission_queue_.empty())
-        {
-            mission_queue_ = {};
-            current_mission_.reset();
-        }
-
-        for (const auto& mission : missions)
-            mission_queue_.push(mission);
-
-        // FIX #9: Transition to QUEUED from any "inactive" state, including
-        //         COMPLETED and QUEUED itself, so the executor always re-arms.
-        if (!missions.empty())
-        {
-            switch (state_)
-            {
-                case MissionState::IDLE:
-                case MissionState::COMPLETED:
-                case MissionState::FAILED:
-                case MissionState::CANCELLED:
-                case MissionState::CLEAR_EMERGENCY:
-                    state_ = MissionState::QUEUED;
-                    break;
-                default:
-                    break; // RUNNING / PAUSED / QUEUED — already active, leave as-is
-            }
-        }
-    }
-
-    std::shared_ptr<Mission> MissionManager::nextMission()
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-
-        if (state_ == MissionState::IDLE      ||
-            state_ == MissionState::PAUSED    ||
-            state_ == MissionState::FAILED    ||
-            state_ == MissionState::CANCELLED ||
-            state_ == MissionState::EMERGENCY ||
-            state_ == MissionState::CLEAR_EMERGENCY) 
-        {
-            return nullptr;
-        }
-
-        // FIX #2: Removed the redundant `if(current_mission_ == nullptr)` check.
-        //         The two guards are now a clean early-return then fall-through.
-        if (current_mission_)
-            return current_mission_;
-
-        if (mission_queue_.empty())
-            return nullptr;
-
-        current_mission_ = mission_queue_.front();
-        mission_queue_.pop();
-        state_ = MissionState::RUNNING;
-
-        return current_mission_;
-    }
-
-    void MissionManager::onNavigationDone()
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-
-        if (state_ != MissionState::RUNNING)
-            return;
-
-        current_mission_.reset();
-        state_ = mission_queue_.empty() ? MissionState::IDLE : MissionState::QUEUED;
-    }
-
-    void MissionManager::onNavigationFailed()
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-
-        if (state_ != MissionState::RUNNING)
-            return;
-
-        current_mission_.reset();
-        while (!mission_queue_.empty()) mission_queue_.pop();
-        state_ = MissionState::FAILED;
-    }
-
-    void MissionManager::cancel()
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        current_mission_.reset();
-        while (!mission_queue_.empty()) mission_queue_.pop();
-        if(state_ == MissionState::EMERGENCY) return;
-        state_ = MissionState::CANCELLED;
-    }
-
-    void MissionManager::emergency()
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        current_mission_.reset();
-        while (!mission_queue_.empty()) mission_queue_.pop();
-        state_ = MissionState::EMERGENCY;
-    }
-
-    void MissionManager::clearEmergency()
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        if(state_ == MissionState::EMERGENCY)
-            state_ = MissionState::CLEAR_EMERGENCY;
-    }
-
-    void MissionManager::pause()
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        if (state_ == MissionState::IDLE    ||
-            state_ == MissionState::RUNNING ||
-            state_ == MissionState::QUEUED)
-            state_ = MissionState::PAUSED;
-    }
-
-    void MissionManager::resume()
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        if (state_ != MissionState::PAUSED) return;
-
-        if (current_mission_)
-            state_ = MissionState::RUNNING;
-        else if (!current_mission_ && !mission_queue_.empty())
-            state_ = MissionState::QUEUED;
-        else
-            state_ = MissionState::IDLE;
-    }
-
-    MissionState MissionManager::state() const
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        return state_;
-    }
-
-    // FIX #3: hasMission now reflects ALL pending work, not just the queue.
-    bool MissionManager::hasMission() const
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        return current_mission_ != nullptr || !mission_queue_.empty();
-    }
-
-    // ─────────────────────────────────────────────────────────────────────────
-    // EventProcessor
-    // ─────────────────────────────────────────────────────────────────────────
-
-    EventProcessor::EventProcessor(MissionManager& mission_manager)
-        : mission_manager_(mission_manager)
-    {}
-
-    EventProcessor::~EventProcessor() { stop(); }
-
-    void EventProcessor::start()
-    {
-        if (running_) return;
-        running_ = true;
-        event_bus_.reset();
-        worker_ = std::thread(&EventProcessor::spin, this);
-    }
-
-    void EventProcessor::stop()
-    {
-        running_ = false;
-        event_bus_.stop();
-        if (worker_.joinable()) worker_.join();
-    }
-
-    void EventProcessor::spin()
-    {
-        Event event;
-        while (running_)
-        {
-            if (event_bus_.pop(event))
-                process(event);
-        }
-    }
-
-    void EventProcessor::process(const Event& event)
-    {
-        switch (event.type)
-        {
-            case EventType::SUBMIT_MISSIONS: mission_manager_.submit(event.missions);      break;
-            case EventType::NAV_DONE:        mission_manager_.onNavigationDone();          break;
-            case EventType::NAV_FAILED:      mission_manager_.onNavigationFailed();        break;
-            case EventType::PAUSE:           mission_manager_.pause();                     break;
-            case EventType::RESUME:          mission_manager_.resume();                    break;
-            case EventType::CANCEL:          mission_manager_.cancel();                    break;
-            case EventType::EMERGENCY:       mission_manager_.emergency();                 break;
-            case EventType::CLEAR_EMERGENCY: mission_manager_.clearEmergency();            break;
-            default:
-                robot::log_error("Unknown event type");
-                break;
-        }
-    }
-
-    void EventProcessor::orderEvent(const robot_protocol_msgs::Order& order)
-    {
-        Event event;
-        event.type     = EventType::SUBMIT_MISSIONS;
-        event.priority = PRIORITY_ORDER;
-        event.missions = vda5050_adapter_.convert(order);
-        event_bus_.push(event);
-    }
-
-    void EventProcessor::goalEvent(const robot_geometry_msgs::PoseStamped& goal)
-    {
-        Event event;
-        event.type     = EventType::SUBMIT_MISSIONS;
-        event.priority = PRIORITY_ORDER;
-        event.missions = goal_adapter_.convert(goal);
-        event_bus_.push(event);
-    }
-
-    void EventProcessor::pauseEvent()
-    {
-        Event event;
-        event.type = EventType::PAUSE;
-        event.priority = PRIORITY_PAUSE;
-        event_bus_.push(std::move(event));
-    }
-
-    void EventProcessor::resumeEvent()
-    {
-        Event event;
-        event.type = EventType::RESUME;
-        event.priority = PRIORITY_RESUME;
-        event_bus_.push(std::move(event));
-    }
-
-    void EventProcessor::cancelEvent()
-    {
-        Event event;
-        event.type = EventType::CANCEL;
-        event.priority = PRIORITY_CANCEL;
-        event_bus_.push(std::move(event));
-    }
-
-    void EventProcessor::navDoneEvent()
-    {
-        Event event;
-        event.type = EventType::NAV_DONE;
-        event.priority = PRIORITY_NAV_DONE;
-        event_bus_.push(std::move(event));
-    }
-
-    // FIX #4: Use PRIORITY_NAV_FAILED (correct constant name).
-    void EventProcessor::navFailedEvent()
-    {
-        Event event;
-        event.type = EventType::NAV_FAILED;
-        event.priority = PRIORITY_NAV_FAILED;
-        event_bus_.push(std::move(event));
-    }
-
-    void EventProcessor::emergencyEvent()
-    {
-        Event event;
-        event.type = EventType::EMERGENCY;
-        event.priority = PRIORITY_EMERGENCY;
-        event_bus_.push(std::move(event));
-    }
-
-    void EventProcessor::clearEmergencyEvent()
-    {
-        Event event;
-        event.type = EventType::CLEAR_EMERGENCY;
-        event.priority = PRIORITY_EMERGENCY;
-        event_bus_.push(std::move(event));
-    }
-
-    // ─────────────────────────────────────────────────────────────────────────
-    // MissionExecutor
-    // ─────────────────────────────────────────────────────────────────────────
-
-    MissionExecutor::MissionExecutor(MissionManager& manager)
-        : mission_manager_(manager)
-    {}
-
-    MissionExecutor::~MissionExecutor() { stop(); }
-
-    void MissionExecutor::start()
-    {
-        if (running_) return;
-        running_ = true;
-        worker_  = std::thread(&MissionExecutor::spin, this);
-    }
-
-    void MissionExecutor::stop()
-    {
-        running_ = false;
-        if (worker_.joinable()) worker_.join();
-    }
-
-    void MissionExecutor::spin()
-    {
-        robot::Rate rate(20);
-
-        while (running_)
-        {
-            auto mission = mission_manager_.nextMission();
-
-            if (mission)
-            {
-                bool is_new_mission = false;
-
-                {
-                    std::lock_guard<std::mutex> lock(mutex_);
-                    if (mission != last_dispatched_mission_)
-                    {
-                        last_dispatched_mission_ = mission;
-                        mission_execute_         = mission;
-                        is_new_mission           = true;
-                    }
-                }
-
-                if (is_new_mission)
-                {
-                    MissionCallback callback;
-                    {
-                        std::lock_guard<std::mutex> lock(mutex_);
-                        callback = mission_callback_;
-                    }
-                    if (callback) callback(mission);
-                }
-            }
-            else
-            {
-                // FIX #5: Only reset the dedup token when the manager is truly
-                //         done (IDLE / CANCELLED / FAILED / EMERGENCY), NOT when
-                //         it is merely PAUSED — the mission hasn't changed there.
-                auto s = mission_manager_.state();
-                if (s == MissionState::IDLE      ||
-                    s == MissionState::CANCELLED  ||
-                    s == MissionState::FAILED     ||
-                    s == MissionState::EMERGENCY)
-                {
-                    std::lock_guard<std::mutex> lock(mutex_);
-                    last_dispatched_mission_.reset();
-                }
-            }
-
-            rate.sleep();
-        }
-    }
-
-} // namespace mission_adapters

src/mission_config.cpp

@@ -0,0 +1,44 @@
+#include <mission_adapters/mission_config.h>
+
+#include <cmath>
+
+#include <robot/robot.h>
+
+namespace mission_adapters
+{
+    bool MissionConfig::loadFromParams(robot::NodeHandle& nh, const std::string& ns)
+    {
+        const std::string prefix = ns.empty() ? std::string() : ns + "/";
+
+        nh.getParam(prefix + "mission_timeout", mission_timeout, mission_timeout);
+        nh.getParam(prefix + "clear_queue_on_failure", clear_queue_on_failure,
+                    clear_queue_on_failure);
+
+        if (!validate())
+            return false;
+
+        print();
+        return true;
+    }
+
+    bool MissionConfig::validate() const
+    {
+        if (!std::isfinite(mission_timeout) || mission_timeout < 0.0)
+        {
+            robot::log_error("MissionConfig: mission_timeout = %.3f is invalid (needs >= 0 [s], 0 "
+                             "= off)", mission_timeout);
+            return false;
+        }
+
+        return true;
+    }
+
+    void MissionConfig::print() const
+    {
+        robot::log_info("MissionConfig: mission_timeout = %.3f [s] (%s), clear_queue_on_failure = %s",
+                        mission_timeout,
+                        mission_timeout > 0.0 ? "on" : "off",
+                        clear_queue_on_failure ? "true" : "false");
+    }
+
+} // namespace mission_adapters

src/mission_executor.cpp

@@ -0,0 +1,80 @@
+#include <mission_adapters/mission_executor.h>
+
+#include <robot/robot.h>
+
+namespace mission_adapters
+{
+    // ─────────────────────────────────────────────────────────────────────────
+    // MissionExecutor
+    // ─────────────────────────────────────────────────────────────────────────
+
+    MissionExecutor::MissionExecutor(MissionManager& manager)
+        : mission_manager_(manager)
+    {}
+
+    MissionExecutor::~MissionExecutor() { stop(); }
+
+    void MissionExecutor::setNavigationClient(NavigationClient* client)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        navigation_client_ = client;
+    }
+
+    void MissionExecutor::start()
+    {
+        if (running_) return;
+        running_ = true;
+        worker_  = std::thread(&MissionExecutor::spin, this);
+    }
+
+    void MissionExecutor::stop()
+    {
+        running_ = false;
+        mission_manager_.wakeUp();   // gỡ thread ra khỏi waitForWork() để join() không treo
+        if (worker_.joinable()) worker_.join();
+    }
+
+    void MissionExecutor::step()
+    {
+        NavigationClient* client = nullptr;
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            client = navigation_client_;
+        }
+
+        if (!client)
+            return;
+
+        // Cancel trước dispatch: nếu order mới vừa thay order cũ, robot phải được bảo dừng chặng cũ
+        // trước khi nhận chặng mới. Đảo thứ tự sẽ có lúc hai lệnh cùng hiệu lực.
+        const MissionId cancel_id = mission_manager_.takePendingCancel();
+        if (cancel_id != kInvalidMissionId)
+            client->cancelActive(cancel_id);
+
+        const auto mission = mission_manager_.nextMission();
+        if (!mission)
+            return;
+
+        if (!client->dispatch(mission))
+        {
+            // Navigation từ chối: coi như chặng này hỏng, đừng để mission kẹt RUNNING vĩnh viễn.
+            robot::log_error("MissionExecutor: navigation rejected mission %lu",
+                             static_cast<unsigned long>(mission->id));
+            mission_manager_.onNavigationFailed(mission->id);
+        }
+    }
+
+    void MissionExecutor::spin()
+    {
+        while (running_)
+        {
+            // M10: chờ đúng lúc có việc thay vì poll. Poll 20 Hz nghĩa là mỗi lệnh cancel phải
+            // đợi trung bình 25 ms trước khi tới được navigation, không vì lý do gì cả.
+            if (!mission_manager_.waitForWork())
+                continue;
+
+            step();
+        }
+    }
+
+} // namespace mission_adapters

src/mission_manager.cpp

@@ -0,0 +1,389 @@
+#include <mission_adapters/mission_manager.h>
+
+#include <chrono>
+
+#include <robot/robot.h>
+
+namespace mission_adapters
+{
+namespace
+{
+    /**
+     * @brief Đánh thức thread đang chờ việc khi rời scope, kể cả khi hàm return sớm.
+     *
+     * Khai báo ngay sau std::lock_guard nên nó bị huỷ TRƯỚC lock_guard: notify xảy ra khi vẫn còn
+     * giữ khoá — an toàn, và không có đường nào ra khỏi hàm mà quên đánh thức executor.
+     */
+    class NotifyOnExit
+    {
+    public:
+        explicit NotifyOnExit(std::condition_variable& cv) : cv_(cv) {}
+        ~NotifyOnExit() { cv_.notify_all(); }
+
+        NotifyOnExit(const NotifyOnExit&) = delete;
+        NotifyOnExit& operator=(const NotifyOnExit&) = delete;
+
+    private:
+        std::condition_variable& cv_;
+    };
+
+}  // namespace
+
+    // ─────────────────────────────────────────────────────────────────────────
+    // MissionManager
+    // ─────────────────────────────────────────────────────────────────────────
+
+    void MissionManager::setConfig(const MissionConfig& config)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+        config_ = config;
+    }
+
+    bool MissionManager::hasWorkLocked() const
+    {
+        return pending_cancel_ != kInvalidMissionId ||
+               (state_ == MissionState::QUEUED && !mission_queue_.empty());
+    }
+
+    bool MissionManager::timeoutArmedLocked() const
+    {
+        return config_.mission_timeout > 0.0 &&
+               state_ == MissionState::RUNNING &&
+               current_mission_ != nullptr;
+    }
+
+    MissionId MissionManager::currentIdLocked() const
+    {
+        return current_mission_ ? current_mission_->id : kInvalidMissionId;
+    }
+
+    void MissionManager::setStateLocked(MissionState next, const char* reason, MissionId id)
+    {
+        if (state_ == next)
+            return;   // C11: chỉ log khi state đổi thật, không log lặp trong vòng lặp
+
+        robot::log_info("MissionManager: %s -> %s (%s, mission %lu)",
+                        toString(state_), toString(next), reason,
+                        static_cast<unsigned long>(id));
+
+        state_ = next;
+    }
+
+    void MissionManager::expireCurrentLocked()
+    {
+        if (!current_mission_)
+            return;
+
+        robot::log_warning("MissionManager: mission %lu exceeded %.3f [s] — cancelling the leg",
+                           static_cast<unsigned long>(current_mission_->id),
+                           config_.mission_timeout);
+
+        // Khác NAV_FAILED: navigation vẫn đang chạy chặng này nên phải bảo nó dừng, nếu không robot
+        // tiếp tục đi tới goal của một mission mà mission layer đã bỏ.
+        const MissionId expired_id = currentIdLocked();
+        requestCancelOfCurrentLocked();
+
+        if (config_.clear_queue_on_failure)
+            while (!mission_queue_.empty()) mission_queue_.pop();
+
+        setStateLocked(mission_queue_.empty() ? MissionState::FAILED : MissionState::QUEUED,
+                       "mission_timeout", expired_id);
+    }
+
+    void MissionManager::requestCancelOfCurrentLocked()
+    {
+        if (!current_mission_)
+            return;
+
+        pending_cancel_ = current_mission_->id;
+        current_mission_.reset();
+    }
+
+    void MissionManager::submit(const std::vector<std::shared_ptr<Mission>>& missions)
+    {
+        // A1: rỗng = không có việc, KHÔNG phải lệnh xoá hàng đợi. Kiểm tra trước khi lấy khoá để
+        // một order lỗi không bao giờ chạm được vào trạng thái.
+        if (missions.empty())
+            return;
+
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+
+        if (state_ == MissionState::EMERGENCY)
+        {
+            robot::log_warning("MissionManager: rejecting submit while in EMERGENCY");
+            return;
+        }
+
+        // Q1 — preempt tường minh: order mới thay order cũ thì chặng đang chạy phải được dừng,
+        // không chỉ bị quên đi.
+        requestCancelOfCurrentLocked();
+
+        mission_queue_ = {};
+
+        for (const auto& mission : missions)
+        {
+            if (!mission)
+                continue;
+
+            mission->id = next_mission_id_++;
+            mission_queue_.push(mission);
+        }
+
+        if (mission_queue_.empty())
+            return;
+
+        // PAUSED được giữ nguyên: người vận hành đã chủ động dừng, order mới không được tự chạy.
+        if (state_ != MissionState::PAUSED)
+            setStateLocked(MissionState::QUEUED, "submit", mission_queue_.back()->id);
+    }
+
+    void MissionManager::append(const std::vector<std::shared_ptr<Mission>>& missions)
+    {
+        if (missions.empty())
+            return;
+
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+
+        if (state_ == MissionState::EMERGENCY)
+        {
+            robot::log_warning("MissionManager: rejecting append while in EMERGENCY");
+            return;
+        }
+
+        for (const auto& mission : missions)
+        {
+            if (!mission)
+                continue;
+
+            mission->id = next_mission_id_++;
+            mission_queue_.push(mission);
+        }
+
+        // RUNNING / PAUSED / QUEUED giữ nguyên: đây là phần nối tiếp, không phải yêu cầu mới.
+        switch (state_)
+        {
+            case MissionState::IDLE:
+            case MissionState::COMPLETED:
+            case MissionState::FAILED:
+            case MissionState::CANCELLED:
+            case MissionState::CLEAR_EMERGENCY:
+                setStateLocked(MissionState::QUEUED, "append", mission_queue_.back()->id);
+                break;
+            default:
+                break;
+        }
+    }
+
+    std::shared_ptr<const Mission> MissionManager::nextMission()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+
+        // M6: chỉ QUEUED mới sinh ra mission mới. RUNNING nghĩa là chặng hiện tại chưa xong nên
+        // không có gì để giao thêm — dedup nằm ở đây, bên gọi không phải tự nhớ.
+        if (state_ != MissionState::QUEUED)
+            return nullptr;
+
+        if (mission_queue_.empty())
+        {
+            setStateLocked(MissionState::IDLE, "queue is empty");
+            return nullptr;
+        }
+
+        current_mission_ = mission_queue_.front();
+        mission_queue_.pop();
+        setStateLocked(MissionState::RUNNING, "dequeue", current_mission_->id);
+        mission_start_time_ = robot::Time::now();   // mốc tính mission_timeout
+
+        return current_mission_;
+    }
+
+    bool MissionManager::onNavigationDone(MissionId id)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+
+        if (state_ != MissionState::RUNNING || !current_mission_)
+            return false;
+
+        // A2: outcome trễ của một mission đã bị thay thế không được tính cho mission đang chạy.
+        if (id != current_mission_->id)
+        {
+            robot::log_warning("MissionManager: dropping NAV_DONE of mission %lu (currently "
+                               "running %lu)",
+                               static_cast<unsigned long>(id),
+                               static_cast<unsigned long>(current_mission_->id));
+            return false;
+        }
+
+        current_mission_.reset();
+        setStateLocked(mission_queue_.empty() ? MissionState::COMPLETED : MissionState::QUEUED,
+                       "nav_done", id);
+        return true;
+    }
+
+    bool MissionManager::onNavigationFailed(MissionId id)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+
+        if (state_ != MissionState::RUNNING || !current_mission_)
+            return false;
+
+        if (id != current_mission_->id)
+        {
+            robot::log_warning("MissionManager: dropping NAV_FAILED of mission %lu (currently "
+                               "running %lu)",
+                               static_cast<unsigned long>(id),
+                               static_cast<unsigned long>(current_mission_->id));
+            return false;
+        }
+
+        current_mission_.reset();
+
+        // M8: mặc định xoá sạch hàng đợi. Với tuyến đường tuần tự, chặng n hỏng nghĩa là robot
+        // không tới được node n — chạy tiếp chặng n+1 là cắt ngang đoạn chưa được cho phép đi.
+        if (config_.clear_queue_on_failure)
+            while (!mission_queue_.empty()) mission_queue_.pop();
+
+        setStateLocked(mission_queue_.empty() ? MissionState::FAILED : MissionState::QUEUED,
+                       "nav_failed", id);
+        return true;
+    }
+
+    void MissionManager::cancel()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+
+        // A3: huỷ phải dừng được robot, không chỉ xoá hàng đợi trong bộ nhớ.
+        const MissionId cancelled_id = currentIdLocked();
+        requestCancelOfCurrentLocked();
+        while (!mission_queue_.empty()) mission_queue_.pop();
+
+        if (state_ == MissionState::EMERGENCY) return;
+        setStateLocked(MissionState::CANCELLED, "cancel", cancelled_id);
+    }
+
+    void MissionManager::emergency()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+
+        const MissionId stopped_id = currentIdLocked();
+        requestCancelOfCurrentLocked();
+        while (!mission_queue_.empty()) mission_queue_.pop();
+        setStateLocked(MissionState::EMERGENCY, "emergency", stopped_id);
+    }
+
+    void MissionManager::clearEmergency()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+
+        if (state_ == MissionState::EMERGENCY)
+            setStateLocked(MissionState::CLEAR_EMERGENCY, "clear_emergency");
+    }
+
+    void MissionManager::pause()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+
+        if (state_ == MissionState::IDLE    ||
+            state_ == MissionState::RUNNING ||
+            state_ == MissionState::QUEUED)
+            setStateLocked(MissionState::PAUSED, "pause", currentIdLocked());
+    }
+
+    void MissionManager::resume()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        NotifyOnExit notify(work_cv_);
+
+        if (state_ != MissionState::PAUSED) return;
+
+        if (current_mission_)
+            setStateLocked(MissionState::RUNNING, "resume", current_mission_->id);
+        else if (!mission_queue_.empty())
+            setStateLocked(MissionState::QUEUED, "resume");
+        else
+            setStateLocked(MissionState::IDLE, "resume");
+    }
+
+    MissionState MissionManager::state() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return state_;
+    }
+
+    // FIX #3: hasMission now reflects ALL pending work, not just the queue.
+    bool MissionManager::hasMission() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return current_mission_ != nullptr || !mission_queue_.empty();
+    }
+
+    MissionId MissionManager::currentMissionId() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return current_mission_ ? current_mission_->id : kInvalidMissionId;
+    }
+
+    MissionId MissionManager::takePendingCancel()
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        const MissionId id = pending_cancel_;
+        pending_cancel_ = kInvalidMissionId;
+        return id;
+    }
+
+    bool MissionManager::waitForWork()
+    {
+        std::unique_lock<std::mutex> lock(mutex_);
+
+        while (true)
+        {
+            if (wake_requested_)
+            {
+                wake_requested_ = false;
+                return false;
+            }
+
+            if (hasWorkLocked())
+                return true;
+
+            if (!timeoutArmedLocked())
+            {
+                work_cv_.wait(lock);
+                continue;
+            }
+
+            // M7: đo bằng robot::Time, không đếm chu kỳ — số chu kỳ không nói gì về thời gian thật
+            // khi vòng lặp bị nghẽn.
+            const double remaining =
+                (mission_start_time_ + robot::Duration(config_.mission_timeout) - robot::Time::now())
+                    .toSec();
+
+            if (remaining <= 0.0)
+            {
+                expireCurrentLocked();
+                continue;
+            }
+
+            work_cv_.wait_for(lock, std::chrono::duration<double>(remaining));
+        }
+    }
+
+    void MissionManager::wakeUp()
+    {
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            wake_requested_ = true;
+        }
+        work_cv_.notify_all();
+    }
+
+} // namespace mission_adapters

src/plugin_registry.cpp

@@ -0,0 +1,180 @@
+#include <mission_adapters/plugin_registry.h>
+
+#include <utility>
+
+#include <boost/dll/import.hpp>
+#include <boost/system/system_error.hpp>
+#include <yaml-cpp/yaml.h>
+
+#include <robot/robot.h>
+
+namespace mission_adapters
+{
+    PluginRegistry::~PluginRegistry()
+    {
+        clear();
+    }
+
+    void PluginRegistry::clear()
+    {
+        // Adapter phải chết trước factory: factory là thứ giữ .so còn nạp, thả ngược thứ tự sẽ gỡ
+        // thư viện trong khi vẫn còn object của nó.
+        adapters_.clear();
+        factories_.clear();
+    }
+
+    bool PluginRegistry::registerAdapter(const MissionSourceAdapter::Ptr& adapter)
+    {
+        if (!adapter)
+        {
+            robot::log_error("PluginRegistry: adapter null");
+            return false;
+        }
+
+        const std::string schema_name = adapter->schema();
+        if (schema_name.empty())
+        {
+            robot::log_error("PluginRegistry: adapter declares an empty schema");
+            return false;
+        }
+
+        const auto existing = adapters_.find(schema_name);
+        if (existing != adapters_.end())
+        {
+            // Hai nguồn cùng schema thì việc định tuyến trở nên phụ thuộc thứ tự nạp — từ chối thay
+            // vì im lặng ghi đè.
+            robot::log_error("PluginRegistry: schema '%s' is already registered by another adapter",
+                             schema_name.c_str());
+            return false;
+        }
+
+        adapters_.emplace(schema_name, adapter);
+        return true;
+    }
+
+    MissionSourceAdapter* PluginRegistry::find(const std::string& schema) const
+    {
+        const auto it = adapters_.find(schema);
+        return it == adapters_.end() ? nullptr : it->second.get();
+    }
+
+    std::vector<std::string> PluginRegistry::schemas() const
+    {
+        std::vector<std::string> result;
+        result.reserve(adapters_.size());
+        for (const auto& entry : adapters_)
+            result.push_back(entry.first);
+
+        return result;
+    }
+
+    bool PluginRegistry::loadOne(const std::string& name, const std::string& type,
+                                 robot::NodeHandle& nh)
+    {
+        robot::PluginLoaderHelper loader(nh);
+        const std::string library_path = loader.findLibraryPath(type);
+
+        if (library_path.empty())
+        {
+            robot::log_error("PluginRegistry: no library found for '%s' — check the key "
+                             "'%s/library_path' in the YAML and that the .so file exists",
+                             type.c_str(), type.c_str());
+            return false;
+        }
+
+        std::function<MissionSourceAdapter::Ptr()> factory;
+
+        try
+        {
+            factory = boost::dll::import_alias<MissionSourceAdapter::Ptr()>(
+                library_path, type, boost::dll::load_mode::append_decorations);
+        }
+        catch (const boost::system::system_error& ex)
+        {
+            // Sai tên symbol hoặc file không nạp được. Bắt tại đây để một plugin hỏng không giết cả
+            // tiến trình — nhưng vẫn báo lỗi để không ai tưởng nguồn này đang chạy.
+            robot::log_error("PluginRegistry: could not load symbol '%s' from '%s': %s",
+                             type.c_str(), library_path.c_str(), ex.what());
+            return false;
+        }
+        catch (const std::exception& ex)
+        {
+            robot::log_error("PluginRegistry: error while loading '%s': %s",
+                             type.c_str(), ex.what());
+            return false;
+        }
+
+        MissionSourceAdapter::Ptr adapter;
+        try
+        {
+            adapter = factory();
+        }
+        catch (const std::exception& ex)
+        {
+            robot::log_error("PluginRegistry: factory of '%s' threw an exception: %s",
+                             type.c_str(), ex.what());
+            return false;
+        }
+
+        if (!adapter)
+        {
+            robot::log_error("PluginRegistry: factory of '%s' returned null", type.c_str());
+            return false;
+        }
+
+        if (!adapter->configure(name, nh))
+        {
+            robot::log_error("PluginRegistry: '%s' (instance '%s') configure() failed",
+                             type.c_str(), name.c_str());
+            return false;
+        }
+
+        if (!registerAdapter(adapter))
+            return false;
+
+        // Chỉ giữ factory sau khi adapter đã vào bảng: nếu đăng ký hỏng thì cũng không giữ .so lại.
+        factories_.push_back(std::move(factory));
+
+        robot::log_info("PluginRegistry: loaded '%s' (instance '%s') for schema '%s'",
+                        type.c_str(), name.c_str(), adapter->schema().c_str());
+        return true;
+    }
+
+    bool PluginRegistry::loadFromConfig(robot::NodeHandle& nh, const std::string& ns)
+    {
+        const std::string key = ns.empty() ? std::string("mission_sources")
+                                           : ns + "/mission_sources";
+
+        YAML::Node sources;
+        if (!nh.getParam(key, sources) || !sources.IsSequence() || sources.size() == 0)
+        {
+            robot::log_error("PluginRegistry: '%s' is missing or is not a list — no mission source "
+                             "was loaded", key.c_str());
+            return false;
+        }
+
+        bool all_ok = true;
+
+        for (size_t i = 0; i < sources.size(); ++i)
+        {
+            const YAML::Node& entry = sources[i];
+
+            if (!entry.IsMap() || !entry["type"])
+            {
+                robot::log_error("PluginRegistry: '%s[%zu]' is missing the 'type' key",
+                                 key.c_str(), i);
+                all_ok = false;
+                continue;
+            }
+
+            const std::string type = entry["type"].as<std::string>();
+            const std::string name = entry["name"] ? entry["name"].as<std::string>() : type;
+
+            if (!loadOne(name, type, nh))
+                all_ok = false;
+        }
+
+        return all_ok;
+    }
+
+} // namespace mission_adapters

src/robot_control_test.cpp

@@ -1,103 +0,0 @@
-#include <mission_adapters/mission_adapters.h>
-#include <move_base_core/navigation.h>
-
-#include <utility>
-
-using namespace mission_adapters;
-
-class RobotControlTest
-{
-    robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
-
-    mission_adapters::MissionManager  mission_manager_;
-    mission_adapters::EventProcessor  event_processor_{mission_manager_};
-    mission_adapters::MissionExecutor mission_executor_{mission_manager_};
-
-    // FIX #7: Fully qualified namespace for the initial value.
-    robot::move_base_core::State prev_nav_state_ = robot::move_base_core::State::PENDING;
-
-    // Tracks whether the actions of the current mission are complete.
-    // FIX #8: Stub — replace with real action-done check from your action executor.
-    bool areActionsDone() const
-    {
-        // TODO: query your action executor for completion status.
-        return true;
-    }
-
-public:
-    explicit RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base);
-    ~RobotControlTest();
-
-    void run();
-
-private:
-    void executeMission(const Mission& mission);
-};
-
-RobotControlTest::RobotControlTest(robot::move_base_core::BaseNavigation::Ptr move_base)
-    : move_base_ptr_(std::move(move_base))
-{}
-
-RobotControlTest::~RobotControlTest()
-{
-    event_processor_.stop();
-    mission_executor_.stop();
-}
-
-void RobotControlTest::run()
-{
-    if (!move_base_ptr_)
-    {
-        robot::log_error("RobotControlTest requires a valid BaseNavigation pointer");
-        return;
-    }
-
-    robot::Rate rate(50);
-
-    mission_executor_.setMissionCallback(
-        [this](const std::shared_ptr<Mission>& mission)
-        {
-            executeMission(*mission);
-        });
-
-    event_processor_.start();
-    mission_executor_.start();
-
-    while (robot::ok())
-    {
-        auto feedback = move_base_ptr_->getFeedback();
-        if (!feedback)
-        {
-            rate.sleep();
-            continue;
-        }
-
-        auto nav_state = feedback->navigation_state;
-
-        if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::SUCCEEDED && areActionsDone())
-        {
-            event_processor_.navDoneEvent();
-        }
-        else if (nav_state != prev_nav_state_ && nav_state == robot::move_base_core::State::ABORTED)
-        {
-            event_processor_.navFailedEvent();
-        }
-        prev_nav_state_ = nav_state;
-
-        // Example: receive an order (replace condition with your real source)
-        if (/* new order available */ false)
-        {
-            robot_protocol_msgs::Order order;
-            // ... populate order ...
-            event_processor_.orderEvent(order);
-        }
-
-        rate.sleep();
-    }
-}
-
-void RobotControlTest::executeMission(const Mission& mission)
-{
-    // TODO: send mission goal to move_base_ptr_
-    (void)mission;
-}

src/types.cpp

@@ -0,0 +1,23 @@
+#include <mission_adapters/types.h>
+
+namespace mission_adapters
+{
+    const char* toString(MissionState state)
+    {
+        switch (state)
+        {
+            case MissionState::IDLE:            return "IDLE";
+            case MissionState::QUEUED:          return "QUEUED";
+            case MissionState::RUNNING:         return "RUNNING";
+            case MissionState::PAUSED:          return "PAUSED";
+            case MissionState::COMPLETED:       return "COMPLETED";
+            case MissionState::FAILED:          return "FAILED";
+            case MissionState::CANCELLED:       return "CANCELLED";
+            case MissionState::EMERGENCY:       return "EMERGENCY";
+            case MissionState::CLEAR_EMERGENCY: return "CLEAR_EMERGENCY";
+        }
+
+        return "UNKNOWN";
+    }
+
+} // namespace mission_adapters

test/adapter_test.cpp

@@ -0,0 +1,527 @@
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+
+#include <cmath>
+#include <string>
+
+#include <mission_adapters/mission_request.h>
+
+#include "goal_source_adapter.h"
+#include "mission_test_utils.h"
+#include "vda5050_source_adapter.h"
+
+namespace
+{
+using namespace mission_adapters;
+using mission_plugins::GoalSourceAdapter;
+using mission_plugins::VDA5050SourceAdapter;
+using mission_test::makeAction;
+using mission_test::makeGoal;
+using mission_test::makeOrder;
+
+class AdapterTest : public ::testing::Test
+{
+protected:
+    void SetUp() override
+    {
+        robot::NodeHandle nh;
+        ASSERT_TRUE(goal_adapter.configure("goal_src", nh));
+        ASSERT_TRUE(order_adapter.configure("vda5050_src", nh));
+    }
+
+    /// Chuyển một order, trả về danh sách mission (bỏ phần mode).
+    std::vector<std::shared_ptr<Mission>> convertOrder(const robot_protocol_msgs::Order& order)
+    {
+        return order_adapter.convert(MissionRequest::fromOrder(order)).missions;
+    }
+
+    /// Chuyển một order và giữ nguyên cả ConversionResult để kiểm mode.
+    ConversionResult convertOrderFull(const robot_protocol_msgs::Order& order)
+    {
+        return order_adapter.convert(MissionRequest::fromOrder(order));
+    }
+
+    GoalSourceAdapter goal_adapter;
+    VDA5050SourceAdapter order_adapter;
+};
+
+// ── Schema ──────────────────────────────────────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, AdaptersDeclareDistinctSchemas)
+{
+    EXPECT_EQ(goal_adapter.schema(), schema::kPoseStamped);
+    EXPECT_EQ(order_adapter.schema(), schema::kVda5050Order);
+    EXPECT_NE(goal_adapter.schema(), order_adapter.schema());
+}
+
+// ── GoalSourceAdapter ───────────────────────────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, GoalAdapterCreatesSingleMission)
+{
+    const auto missions = goal_adapter.convert(MissionRequest::fromPose(makeGoal(5.5, 9.1))).missions;
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->type, MissionType::SIMPLE_GOAL);
+    EXPECT_EQ(missions.front()->motion_hint, "position");
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 5.5);
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 9.1);
+}
+
+TEST_F(AdapterTest, GoalAdapterRejectsMissingPayload)
+{
+    MissionRequest request;
+    request.schema = schema::kPoseStamped;
+
+    std::string reason;
+    EXPECT_FALSE(goal_adapter.validate(request, reason));
+    EXPECT_FALSE(reason.empty());
+}
+
+TEST_F(AdapterTest, GoalAdapterRejectsNonFiniteGoal)
+{
+    auto goal = makeGoal(1.0, 1.0);
+    goal.pose.position.x = std::numeric_limits<double>::quiet_NaN();
+    goal.pose.orientation.w = 1.0;
+
+    std::string reason;
+    EXPECT_FALSE(goal_adapter.validate(MissionRequest::fromPose(goal), reason))
+        << "a goal containing NaN slipped through the boundary into navigation";
+}
+
+TEST_F(AdapterTest, GoalAdapterRejectsZeroQuaternion)
+{
+    auto goal = makeGoal(1.0, 1.0);
+
+    std::string reason;
+    ASSERT_TRUE(goal_adapter.validate(MissionRequest::fromPose(goal), reason)) << reason;
+
+    // Host quên set orientation: quaternion toàn 0 không phải "hướng bất kỳ", nó là dữ liệu hỏng.
+    goal.pose.orientation.w = 0.0;
+    EXPECT_FALSE(goal_adapter.validate(MissionRequest::fromPose(goal), reason));
+}
+
+// ── VDA5050SourceAdapter ────────────────────────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, EmptyOrderIsRejectedByValidate)
+{
+    robot_protocol_msgs::Order order;
+
+    std::string reason;
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(order), reason));
+    EXPECT_TRUE(convertOrder(order).empty());
+}
+
+TEST_F(AdapterTest, OrderWithoutActionsCreatesOneTailMission)
+{
+    const auto missions = convertOrder(makeOrder(4));
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->nodes.size(), 4u);
+    EXPECT_EQ(missions.front()->edges.size(), 3u);
+}
+
+TEST_F(AdapterTest, InvalidOrderWithMissingEdgesIsRejected)
+{
+    auto order = makeOrder(4);
+    order.edges.pop_back();
+
+    std::string reason;
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(order), reason));
+    EXPECT_TRUE(convertOrder(order).empty());
+}
+
+TEST_F(AdapterTest, OrderSplitsAtNodeAction)
+{
+    auto order = makeOrder(5);
+    order.nodes[2].actions.push_back(makeAction("dock"));
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 2u);
+    EXPECT_EQ(missions[0]->nodes.size(), 3u);
+    EXPECT_EQ(missions[1]->nodes.size(), 3u);
+}
+
+TEST_F(AdapterTest, OrderCollectsAndSortsActions)
+{
+    auto order = makeOrder(2);
+    order.edges[0].actions.push_back(makeAction("edge"));
+    order.nodes[1].actions.push_back(makeAction("node"));
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    ASSERT_EQ(missions.front()->actions.size(), 2u);
+    EXPECT_EQ(missions.front()->actions[0].type, ActionType::EDGE_ACTION);
+    EXPECT_EQ(missions.front()->actions[1].type, ActionType::NODE_ACTION);
+}
+
+// ── A4: conformance VDA5050 ─────────────────────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, MissionCarriesGoalAndStartFromNodes)
+{
+    auto order = makeOrder(3);
+    order.nodes[2].nodePosition.x = 7.0;
+    order.nodes[2].nodePosition.y = 8.0;
+    order.nodes[2].nodePosition.theta = 0.0;
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    // Mission phải self-contained: consumer không phải tự đoán goal từ nodes.back().
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 7.0);
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 8.0);
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.orientation.w, 1.0);
+    EXPECT_EQ(missions.front()->goal.header.frame_id, "map");
+
+    EXPECT_DOUBLE_EQ(missions.front()->start.pose.position.x, 0.0);
+}
+
+TEST_F(AdapterTest, NodeThetaBecomesQuaternion)
+{
+    auto order = makeOrder(2);
+    order.nodes[1].nodePosition.theta = M_PI;   // [rad]
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    // theta = pi -> quay 180 độ quanh z: (z, w) = (1, 0).
+    EXPECT_NEAR(missions.front()->goal.pose.orientation.z, 1.0, 1e-9);
+    EXPECT_NEAR(missions.front()->goal.pose.orientation.w, 0.0, 1e-9);
+}
+
+TEST_F(AdapterTest, HorizonNodesAreNeverDispatched)
+{
+    auto order = makeOrder(5);
+    // Chỉ 3 node đầu là base; hai node cuối là horizon (fleet manager chưa cho phép đi).
+    order.nodes[3].released = false;
+    order.nodes[4].released = false;
+    order.edges[3].released = false;
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->nodes.size(), 3u) << "the robot was handed the horizon part that "
+                                                     "is not released yet";
+    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 2.0);
+}
+
+TEST_F(AdapterTest, OrderWithoutReleasedFlagIsTreatedAsFullBase)
+{
+    auto order = makeOrder(3);
+    for (auto& node : order.nodes) node.released = false;
+    for (auto& edge : order.edges) edge.released = false;
+
+    // Host không điền `released`: chạy cả order còn hơn đứng im không dấu hiệu.
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_EQ(missions.front()->nodes.size(), 3u);
+}
+
+TEST_F(AdapterTest, StaleOrderUpdateIdIsRejected)
+{
+    ASSERT_FALSE(convertOrder(makeOrder(3, "order_A", 2)).empty());
+
+    std::string reason;
+
+    // Cùng orderId, orderUpdateId cũ hơn -> phát lại/đến trễ, không được chạy lại tuyến đường.
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(3, "order_A", 1)),
+                                        reason));
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(3, "order_A", 2)),
+                                        reason));
+
+    // Cùng orderId, update mới hơn -> chấp nhận.
+    EXPECT_TRUE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(4, "order_A", 3)),
+                                       reason)) << reason;
+
+    // orderId khác -> order mới, không so orderUpdateId.
+    EXPECT_TRUE(order_adapter.validate(MissionRequest::fromOrder(makeOrder(3, "order_B", 0)),
+                                       reason)) << reason;
+}
+
+TEST_F(AdapterTest, OrderUpdateAppendsOnlyNewlyReleasedSegment)
+{
+    // Base ban đầu: 3 node released, 2 node horizon.
+    auto order = makeOrder(5, "order_A", 0);
+    order.nodes[3].released = false;
+    order.nodes[4].released = false;
+    order.edges[3].released = false;
+
+    const auto first = convertOrderFull(order);
+    ASSERT_EQ(first.missions.size(), 1u);
+    EXPECT_EQ(first.mode, SubmitMode::kReplace);
+    EXPECT_EQ(first.missions.front()->nodes.size(), 3u);
+
+    // Fleet manager release nốt horizon.
+    auto update = makeOrder(5, "order_A", 1);
+    const auto second = convertOrderFull(update);
+
+    ASSERT_EQ(second.missions.size(), 1u);
+    EXPECT_EQ(second.mode, SubmitMode::kAppend)
+        << "an order update replaced the whole queue -> the robot cancels and redoes the leg it is "
+           "on";
+
+    // Chặng mới bắt đầu từ node cuối của phần đã chạy, không lặp lại phần cũ.
+    EXPECT_EQ(second.missions.front()->nodes.size(), 3u);
+    EXPECT_DOUBLE_EQ(second.missions.front()->start.pose.position.x, 2.0);
+    EXPECT_DOUBLE_EQ(second.missions.front()->goal.pose.position.x, 4.0);
+}
+
+TEST_F(AdapterTest, OrderUpdateWithoutNewNodesProducesNoWork)
+{
+    ASSERT_FALSE(convertOrder(makeOrder(3, "order_A", 0)).empty());
+
+    // Update không release thêm node nào: không có việc mới, và cũng không được xoá gì.
+    const auto result = convertOrderFull(makeOrder(3, "order_A", 1));
+    EXPECT_TRUE(result.empty());
+}
+
+TEST_F(AdapterTest, NewOrderIdReplacesQueue)
+{
+    ASSERT_FALSE(convertOrder(makeOrder(3, "order_A", 0)).empty());
+
+    const auto result = convertOrderFull(makeOrder(2, "order_B", 0));
+
+    ASSERT_FALSE(result.empty());
+    EXPECT_EQ(result.mode, SubmitMode::kReplace);
+}
+
+// ── D8: goal optional, action đi nguyên vẹn ─────────────────────────────────────────────────────
+
+TEST_F(AdapterTest, ActionAtFirstNodeBecomesGoallessMission)
+{
+    auto order = makeOrder(3);
+    order.nodes[0].actions.push_back(makeAction("pick_at_start"));
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 2u);
+
+    // Chặng đầu: action ngay tại chỗ đứng, không có quãng đường nào để đi.
+    EXPECT_FALSE(missions[0]->has_goal);
+    ASSERT_EQ(missions[0]->actions.size(), 1u);
+    EXPECT_EQ(missions[0]->actions.front().action.actionId, "pick_at_start");
+
+    // Chặng sau: di chuyển bình thường.
+    EXPECT_TRUE(missions[1]->has_goal);
+    EXPECT_DOUBLE_EQ(missions[1]->goal.pose.position.x, 2.0);
+}
+
+TEST_F(AdapterTest, MovingMissionsAlwaysHaveGoal)
+{
+    const auto missions = convertOrder(makeOrder(3));
+
+    ASSERT_EQ(missions.size(), 1u);
+    EXPECT_TRUE(missions.front()->has_goal);
+}
+
+TEST_F(AdapterTest, ActionsPassThroughUnchangedAndInOrder)
+{
+    auto order = makeOrder(3);
+    order.edges[0].sequenceId = 1;
+    order.edges[0].actions.push_back(makeAction("beep"));
+    order.edges[1].sequenceId = 3;
+    order.edges[1].actions.push_back(makeAction("horn"));
+    order.nodes[2].sequenceId = 4;
+    order.nodes[2].actions.push_back(makeAction("lift"));
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u);
+    const auto& actions = missions.front()->actions;
+
+    ASSERT_EQ(actions.size(), 3u);
+    EXPECT_EQ(actions[0].action.actionId, "beep");
+    EXPECT_EQ(actions[1].action.actionId, "horn");
+    EXPECT_EQ(actions[2].action.actionId, "lift");
+
+    // Mission layer không diễn giải actionType — nó chỉ chuyển tiếp.
+    EXPECT_EQ(actions[2].action.actionType, "TEST");
+    EXPECT_EQ(actions[2].type, ActionType::NODE_ACTION);
+}
+
+TEST_F(AdapterTest, NonFiniteNodePositionIsRejected)
+{
+    auto order = makeOrder(3);
+    order.nodes[1].nodePosition.x = std::numeric_limits<double>::infinity();
+
+    std::string reason;
+    EXPECT_FALSE(order_adapter.validate(MissionRequest::fromOrder(order), reason));
+}
+
+}  // namespace
+
+// ── Compound action: mở rộng thành chuỗi chặng ──────────────────────────────────────────────────
+
+namespace
+{
+/// @brief Gắn một action có tham số vào node.
+void addAction(robot_protocol_msgs::Node& node, const std::string& type,
+               const std::string& param_key = "", const std::string& param_value = "")
+{
+    robot_protocol_msgs::Action action;
+    action.actionType = type;
+    action.actionId = type + "_id";
+
+    if (!param_key.empty())
+    {
+        robot_protocol_msgs::ActionParameter p;
+        p.key = param_key;
+        p.value = param_value;
+        action.actionParameters.push_back(p);
+    }
+    node.actions.push_back(action);
+}
+}  // namespace
+
+TEST_F(AdapterTest, CompoundActionExpandsIntoASequenceOfLegs)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "charge", "goal_frame", "charger_02_goal");
+
+    const auto missions = convertOrder(order);
+
+    // nav -> n1 | DetectCharger | docking | startCharging
+    ASSERT_EQ(missions.size(), 4u);
+
+    EXPECT_TRUE(missions[0]->has_goal);
+    EXPECT_TRUE(missions[0]->actions.empty()) << "action compound phải được GỠ khỏi chặng nav";
+    EXPECT_EQ(missions[0]->motion_hint, "position");
+
+    ASSERT_EQ(missions[1]->actions.size(), 1u);
+    EXPECT_EQ(missions[1]->actions[0].action.actionType, "DetectCharger");
+    EXPECT_FALSE(missions[1]->has_goal);
+
+    EXPECT_TRUE(missions[2]->has_goal);
+    EXPECT_EQ(missions[2]->motion_hint, "docking");
+    EXPECT_EQ(missions[2]->goal_frame, "charger_02_goal") << "frame phải lấy từ actionParameters";
+    EXPECT_EQ(missions[2]->marker, "charger") << "marker phải chọn override planner độc lập TF";
+
+    ASSERT_EQ(missions[3]->actions.size(), 1u);
+    EXPECT_EQ(missions[3]->actions[0].action.actionType, "startCharging");
+    EXPECT_FALSE(missions[3]->has_goal);
+}
+
+TEST_F(AdapterTest, GeneratedActionsCarryTheOriginalParameters)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "charge", "goal_frame", "charger_02_goal");
+
+    const auto missions = convertOrder(order);
+    ASSERT_EQ(missions.size(), 4u);
+
+    // Handler dò cần biết dò trạm nào; nó là chỗ duy nhất hiểu ý nghĩa các tham số đó.
+    const auto& detect = missions[1]->actions[0].action;
+    ASSERT_EQ(detect.actionParameters.size(), 1u);
+    EXPECT_EQ(detect.actionParameters[0].key, "goal_frame");
+    EXPECT_EQ(detect.actionParameters[0].value, "charger_02_goal");
+
+    // actionId suy từ id gốc để truy vết được trong log.
+    EXPECT_NE(detect.actionId.find("charge_id"), std::string::npos);
+}
+
+TEST_F(AdapterTest, PlainActionsKeepTheirJsonOrderAroundACompound)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "MutedOn");
+    addAction(order.nodes[1], "charge", "goal_frame", "charger_goal");
+    addAction(order.nodes[1], "MutedOff");
+
+    const auto missions = convertOrder(order);
+
+    // nav+MutedOn | Detect | docking | startCharging | MutedOff
+    ASSERT_EQ(missions.size(), 5u);
+
+    ASSERT_EQ(missions[0]->actions.size(), 1u);
+    EXPECT_EQ(missions[0]->actions[0].action.actionType, "MutedOn");
+
+    // MutedOff phải nằm SAU chuỗi charge. Gom hết action thường vào chặng nav sẽ bật lại cảm biến
+    // an toàn trước khi robot lùi vào trạm — đúng thứ MutedOn sinh ra để tránh.
+    ASSERT_EQ(missions[4]->actions.size(), 1u);
+    EXPECT_EQ(missions[4]->actions[0].action.actionType, "MutedOff");
+    EXPECT_FALSE(missions[4]->has_goal);
+}
+
+TEST_F(AdapterTest, ActionOutsideTheTableRunsAsAPlainAction)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "MutedOn");
+
+    const auto missions = convertOrder(order);
+
+    ASSERT_EQ(missions.size(), 1u) << "action thường không được mở rộng";
+    ASSERT_EQ(missions[0]->actions.size(), 1u);
+    EXPECT_EQ(missions[0]->actions[0].action.actionType, "MutedOn");
+}
+
+TEST_F(AdapterTest, RelativeMoveStepBecomesALegWithADistance)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "UnDockFromStation");
+
+    const auto missions = convertOrder(order);
+    ASSERT_EQ(missions.size(), 2u);
+
+    EXPECT_TRUE(missions[1]->has_goal);
+    EXPECT_EQ(missions[1]->motion_hint, "go_straight");
+    EXPECT_DOUBLE_EQ(missions[1]->relative_distance, -1.0);
+    EXPECT_TRUE(missions[1]->goal_frame.empty());
+}
+
+TEST_F(AdapterTest, FixedFrameStepDoesNotNeedAnyParameter)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "FixedDock");
+
+    const auto missions = convertOrder(order);
+    ASSERT_EQ(missions.size(), 2u);
+    EXPECT_EQ(missions[1]->goal_frame, "dock_target");
+    EXPECT_EQ(missions[1]->marker, "trolley");
+}
+
+TEST_F(AdapterTest, MissingStructuralParameterDropsTheWholeOrder)
+{
+    auto order = makeOrder(2);
+    addAction(order.nodes[1], "charge");   // thiếu goal_frame
+
+    // Bỏ CẢ order: một order chạy nửa vời — robot tới trạm sạc rồi không sạc — nguy hiểm hơn là
+    // không chạy. Và hàng đợi đang chạy không bị đụng tới (A1).
+    EXPECT_TRUE(convertOrder(order).empty());
+}
+
+TEST(CompoundConfig, RejectsAStepWithTwoKeywords)
+{
+    robot::NodeHandle nh;
+    VDA5050SourceAdapter adapter;
+    EXPECT_FALSE(adapter.configure("vda5050_bad_step", nh))
+        << "không đúng một từ khoá thì không có cách diễn giải nào hiển nhiên đúng";
+}
+
+TEST(CompoundConfig, RejectsARecursiveCompound)
+{
+    robot::NodeHandle nh;
+    VDA5050SourceAdapter adapter;
+    EXPECT_FALSE(adapter.configure("vda5050_recursive", nh))
+        << "expander sẽ mở rộng chính output của mình";
+}
+
+TEST(CompoundConfig, RejectsInvalidOrActionOnlyProfiles)
+{
+    robot::NodeHandle nh;
+    VDA5050SourceAdapter adapter;
+    EXPECT_FALSE(adapter.configure("vda5050_bad_profile", nh));
+}
+
+int main(int argc, char** argv)
+{
+    // Bảng `compound_actions` nằm trong cây config test. `overwrite = 0` để shell vẫn override được
+    // khi cần chạy tay với cây khác — cùng cách `plugin_registry_test` làm.
+#ifdef MISSION_ADAPTERS_TEST_CONFIG_DIR
+    setenv("PNKX_NAV_CORE_CONFIG_DIR", MISSION_ADAPTERS_TEST_CONFIG_DIR, 0);
+#endif
+
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

test/config/mission_adapters_params.yaml

@@ -0,0 +1,107 @@
+# Config CHỈ dùng cho test của gói. Bản runtime nằm ở
+# `pnkx_nav_core/config/mission_adapters_params.yaml` (C2) — sửa tham số vận hành thì sửa ở đó.
+#
+# Chạy test kèm: PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/mission_adapters/test/config
+
+mission_adapters:
+  # Nguồn mission. Thêm loại mới chỉ cần thêm một entry ở đây + một plugin .so.
+  mission_sources:
+    - {name: goal_src,    type: GoalSourceAdapter}
+    - {name: vda5050_src, type: VDA5050SourceAdapter}
+
+  mission_timeout: 0.0           # [s] 0 = tắt
+  clear_queue_on_failure: false  # false = chỉ bỏ chặng lỗi, giữ phần còn lại của order
+
+# Bảng symbol -> thư viện cho Boost.DLL. Thiếu khoá 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".
+GoalSourceAdapter:
+  library_path: libmission_adapters_goal_source
+
+VDA5050SourceAdapter:
+  library_path: libmission_adapters_vda5050_source
+
+# ── Các case lỗi mà plugin_registry_test cố ý dựng ra ────────────────────────────────────────────
+#
+# Mỗi case là một namespace riêng để test gọi loadFromConfig(nh, "<namespace>") mà không phải sinh
+# file YAML lúc chạy.
+
+registry_test_missing_library_path:
+  mission_sources:
+    - {name: bad_src, type: MissingLibraryPathAdapter}
+
+registry_test_missing_library_file:
+  mission_sources:
+    - {name: bad_src, type: MissingLibraryFileAdapter}
+
+registry_test_wrong_symbol:
+  mission_sources:
+    - {name: bad_src, type: WrongSymbolAdapter}
+
+registry_test_duplicate_schema:
+  mission_sources:
+    - {name: goal_a, type: GoalSourceAdapter}
+    - {name: goal_b, type: GoalSourceAdapter}
+
+registry_test_entry_without_type:
+  mission_sources:
+    - {name: nameless_src}
+
+registry_test_empty:
+  mission_timeout: 0.0
+
+
+# Khai trong mission_sources nhưng KHÔNG có khoá library_path.
+MissingLibraryPathAdapter:
+  description: "cố ý thiếu library_path"
+
+# library_path trỏ tới file không tồn tại.
+MissingLibraryFileAdapter:
+  library_path: libmission_adapters_does_not_exist
+
+# Thư viện có thật nhưng symbol không tồn tại trong đó.
+WrongSymbolAdapter:
+  library_path: libmission_adapters_goal_source
+
+# ── Compound action: action "phải dò rồi mới biết đích" ─────────────────────────────────────────
+#
+# Bảng chỉ giữ CẤU TRÚC; frame cụ thể tới từ actionParameters của chính order.
+vda5050_src:
+  global_frame: map
+  compound_actions:
+    charge:
+      steps:
+        - {action: DetectCharger}
+        - {move_to_param: goal_frame, profile: docking, marker: charger}
+        - {action: startCharging}
+
+    UnDockFromStation:
+      steps:
+        - {move: -1.0, profile: go_straight}
+
+    FixedDock:
+      steps:
+        - {move_to: dock_target, profile: docking, marker: trolley}
+
+# Step có hai từ khoá -> configure() phải từ chối lúc boot.
+vda5050_bad_step:
+  compound_actions:
+    charge:
+      steps:
+        - {action: DetectCharger, move: -1.0}
+
+# Compound sinh ra một actionType cũng là compound -> đệ quy.
+vda5050_recursive:
+  compound_actions:
+    charge:
+      steps:
+        - {action: PickUp}
+    PickUp:
+      steps:
+        - {move: 0.5}
+
+# Profile chỉ có nghĩa trên step navigation và phải thuộc bốn profile runtime.
+vda5050_bad_profile:
+  compound_actions:
+    invalid_name:
+      steps:
+        - {move: 0.5, profile: dock}

test/event_bus_test.cpp

@@ -0,0 +1,133 @@
+#include <gtest/gtest.h>
+
+#include <thread>
+#include <vector>
+
+#include <mission_adapters/event.h>
+
+namespace
+{
+using namespace mission_adapters;
+
+Event makeEvent(EventType type)
+{
+    Event event;
+    event.type = type;
+    return event;
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// A5 — thứ tự xử lý phải bằng thứ tự phát sinh.
+//
+// Bản trước sắp theo priority toàn phần: CANCEL (1) luôn đứng trước SUBMIT (5). Phát order rồi huỷ
+// ngay thì huỷ được xử lý trước, order vào hàng đợi sau, và robot chạy đúng cái người dùng vừa huỷ.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(EventBusTest, SubmitThenCancelKeepsCausalOrder)
+{
+    EventBus bus;
+
+    bus.push(makeEvent(EventType::SUBMIT_REQUEST));
+    bus.push(makeEvent(EventType::CANCEL));
+
+    Event event;
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::SUBMIT_REQUEST);
+
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::CANCEL) << "the cancel was processed before the order it "
+                                                "cancels";
+}
+
+TEST(EventBusTest, PreservesFifoOrderAcrossEventTypes)
+{
+    EventBus bus;
+
+    const std::vector<EventType> pushed = {
+        EventType::PAUSE,
+        EventType::SUBMIT_REQUEST,
+        EventType::RESUME,
+        EventType::NAV_DONE,
+        EventType::CANCEL,
+    };
+
+    for (const auto type : pushed)
+        bus.push(makeEvent(type));
+
+    for (size_t i = 0; i < pushed.size(); ++i)
+    {
+        Event event;
+        ASSERT_TRUE(bus.pop(event)) << "event number " << i;
+        EXPECT_EQ(event.type, pushed[i]) << "wrong order at position " << i;
+        EXPECT_EQ(event.sequence, i);
+    }
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// Emergency đi ngoài hàng đợi: độ trễ phản ứng không được phụ thuộc độ dài hàng đợi.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(EventBusTest, EmergencyOvertakesQueue)
+{
+    EventBus bus;
+
+    for (int i = 0; i < 50; ++i)
+        bus.push(makeEvent(EventType::SUBMIT_REQUEST));
+
+    EXPECT_FALSE(bus.emergencyPending());
+    bus.pushEmergency(makeEvent(EventType::EMERGENCY));
+
+    // Thấy được ngay, không phải rút hết 50 sự kiện kia ra trước.
+    EXPECT_TRUE(bus.emergencyPending());
+    EXPECT_EQ(bus.size(), 51u) << "an EMERGENCY event must still sit in the queue to keep the log "
+                                  "sequence";
+
+    EXPECT_TRUE(bus.takeEmergency());
+    EXPECT_FALSE(bus.takeEmergency()) << "the emergency flag was consumed twice";
+}
+
+TEST(EventBusTest, EmergencyEventStillArrivesInOrder)
+{
+    EventBus bus;
+
+    bus.push(makeEvent(EventType::PAUSE));
+    bus.pushEmergency(makeEvent(EventType::EMERGENCY));
+
+    Event event;
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::PAUSE);
+
+    ASSERT_TRUE(bus.pop(event));
+    EXPECT_EQ(event.type, EventType::EMERGENCY);
+}
+
+TEST(EventBusTest, StopUnblocksPop)
+{
+    EventBus bus;
+    Event event;
+
+    std::thread stopper([&bus] { bus.stop(); });
+    EXPECT_FALSE(bus.pop(event));
+    stopper.join();
+}
+
+TEST(EventBusTest, ResetDropsPendingEventsAndEmergencyFlag)
+{
+    EventBus bus;
+
+    bus.push(makeEvent(EventType::PAUSE));
+    bus.pushEmergency(makeEvent(EventType::EMERGENCY));
+
+    bus.reset();
+
+    EXPECT_TRUE(bus.empty());
+    EXPECT_FALSE(bus.emergencyPending());
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

test/mission_adapters_test.cpp

@@ -1,360 +0,0 @@
-#include <gtest/gtest.h>
-
-#include <atomic>
-#include <chrono>
-#include <thread>
-
-#include <mission_adapters/mission_adapters.h>
-
-namespace
-{
-using namespace mission_adapters;
-
-robot_geometry_msgs::PoseStamped makeGoal(double x, double y)
-{
-    robot_geometry_msgs::PoseStamped goal;
-    goal.pose.position.x = x;
-    goal.pose.position.y = y;
-    return goal;
-}
-
-robot_protocol_msgs::Action makeAction(const std::string& id)
-{
-    robot_protocol_msgs::Action action;
-    action.actionId = id;
-    action.actionType = "TEST";
-    return action;
-}
-
-robot_protocol_msgs::Node makeNode(int sequence_id, bool add_action = false)
-{
-    robot_protocol_msgs::Node node;
-    node.sequenceId = sequence_id;
-    node.nodeId = "node_" + std::to_string(sequence_id);
-    node.nodePosition.x = sequence_id;
-    node.nodePosition.y = sequence_id;
-
-    if (add_action)
-        node.actions.push_back(makeAction("node_action_" + std::to_string(sequence_id)));
-
-    return node;
-}
-
-robot_protocol_msgs::Edge makeEdge(int sequence_id, bool add_action = false)
-{
-    robot_protocol_msgs::Edge edge;
-    edge.sequenceId = sequence_id;
-    edge.edgeId = "edge_" + std::to_string(sequence_id);
-
-    if (add_action)
-        edge.actions.push_back(makeAction("edge_action_" + std::to_string(sequence_id)));
-
-    return edge;
-}
-
-robot_protocol_msgs::Order makeOrder(int node_count)
-{
-    robot_protocol_msgs::Order order;
-
-    for (int i = 0; i < node_count; ++i)
-        order.nodes.push_back(makeNode(i));
-
-    for (int i = 0; i < node_count - 1; ++i)
-        order.edges.push_back(makeEdge(i));
-
-    return order;
-}
-
-bool waitForState(MissionManager& manager, MissionState expected, std::chrono::milliseconds timeout)
-{
-    const auto deadline = std::chrono::steady_clock::now() + timeout;
-    while (std::chrono::steady_clock::now() < deadline)
-    {
-        if (manager.state() == expected)
-            return true;
-
-        std::this_thread::sleep_for(std::chrono::milliseconds(5));
-    }
-
-    return manager.state() == expected;
-}
-
-class MissionAdaptersTest : public ::testing::Test
-{
-protected:
-    GoalAdapter goal_adapter;
-    VDA5050Adapter order_adapter;
-};
-
-TEST(EventBusTest, PopsHighestPriorityFirst)
-{
-    EventBus bus;
-
-    Event pause;
-    pause.type = EventType::PAUSE;
-    pause.priority = PRIORITY_PAUSE;
-    bus.push(pause);
-
-    Event cancel;
-    cancel.type = EventType::CANCEL;
-    cancel.priority = PRIORITY_CANCEL;
-    bus.push(cancel);
-
-    Event emergency;
-    emergency.type = EventType::EMERGENCY;
-    emergency.priority = PRIORITY_EMERGENCY;
-    bus.push(emergency);
-
-    Event event;
-    ASSERT_TRUE(bus.pop(event));
-    EXPECT_EQ(event.type, EventType::EMERGENCY);
-
-    ASSERT_TRUE(bus.pop(event));
-    EXPECT_EQ(event.type, EventType::CANCEL);
-
-    ASSERT_TRUE(bus.pop(event));
-    EXPECT_EQ(event.type, EventType::PAUSE);
-}
-
-TEST(EventBusTest, PreservesFifoOrderForSamePriority)
-{
-    EventBus bus;
-
-    Event emergency;
-    emergency.type = EventType::EMERGENCY;
-    emergency.priority = PRIORITY_EMERGENCY;
-    bus.push(emergency);
-
-    Event clear_emergency;
-    clear_emergency.type = EventType::CLEAR_EMERGENCY;
-    clear_emergency.priority = PRIORITY_EMERGENCY;
-    bus.push(clear_emergency);
-
-    Event event;
-    ASSERT_TRUE(bus.pop(event));
-    EXPECT_EQ(event.type, EventType::EMERGENCY);
-
-    ASSERT_TRUE(bus.pop(event));
-    EXPECT_EQ(event.type, EventType::CLEAR_EMERGENCY);
-}
-
-TEST(EventBusTest, StopUnblocksPop)
-{
-    EventBus bus;
-    Event event;
-
-    std::thread stopper([&bus] { bus.stop(); });
-    EXPECT_FALSE(bus.pop(event));
-    stopper.join();
-}
-
-TEST(EventBusTest, ResetDropsPendingEvents)
-{
-    EventBus bus;
-
-    Event pause;
-    pause.type = EventType::PAUSE;
-    pause.priority = PRIORITY_PAUSE;
-    bus.push(pause);
-
-    bus.reset();
-    EXPECT_TRUE(bus.empty());
-}
-
-TEST_F(MissionAdaptersTest, GoalAdapterCreatesSingleMission)
-{
-    const auto missions = goal_adapter.convert(makeGoal(5.5, 9.1));
-
-    ASSERT_EQ(missions.size(), 1u);
-    EXPECT_EQ(missions.front()->type, MissionType::SIMPLE_GOAL);
-    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.x, 5.5);
-    EXPECT_DOUBLE_EQ(missions.front()->goal.pose.position.y, 9.1);
-}
-
-TEST_F(MissionAdaptersTest, EmptyOrderCreatesNoMissions)
-{
-    robot_protocol_msgs::Order order;
-    EXPECT_TRUE(order_adapter.convert(order).empty());
-}
-
-TEST_F(MissionAdaptersTest, OrderWithoutActionsCreatesOneTailMission)
-{
-    const auto missions = order_adapter.convert(makeOrder(4));
-
-    ASSERT_EQ(missions.size(), 1u);
-    EXPECT_EQ(missions.front()->nodes.size(), 4u);
-    EXPECT_EQ(missions.front()->edges.size(), 3u);
-}
-
-TEST_F(MissionAdaptersTest, InvalidOrderWithMissingEdgesCreatesNoMissions)
-{
-    auto order = makeOrder(4);
-    order.edges.pop_back();
-
-    EXPECT_TRUE(order_adapter.convert(order).empty());
-}
-
-TEST_F(MissionAdaptersTest, OrderSplitsAtNodeAction)
-{
-    auto order = makeOrder(5);
-    order.nodes[2].actions.push_back(makeAction("dock"));
-
-    const auto missions = order_adapter.convert(order);
-
-    ASSERT_EQ(missions.size(), 2u);
-    EXPECT_EQ(missions[0]->nodes.size(), 3u);
-    EXPECT_EQ(missions[1]->nodes.size(), 3u);
-}
-
-TEST_F(MissionAdaptersTest, OrderCollectsAndSortsActions)
-{
-    auto order = makeOrder(2);
-    order.edges[0].actions.push_back(makeAction("edge"));
-    order.nodes[1].actions.push_back(makeAction("node"));
-
-    const auto missions = order_adapter.convert(order);
-
-    ASSERT_EQ(missions.size(), 1u);
-    ASSERT_EQ(missions.front()->actions.size(), 2u);
-    EXPECT_EQ(missions.front()->actions[0].type, ActionType::EDGE_ACTION);
-    EXPECT_EQ(missions.front()->actions[1].type, ActionType::NODE_ACTION);
-}
-
-TEST_F(MissionAdaptersTest, ManagerRunsMissionLifecycle)
-{
-    MissionManager manager;
-
-    manager.submit(goal_adapter.convert(makeGoal(1.0, 2.0)));
-    EXPECT_EQ(manager.state(), MissionState::QUEUED);
-    EXPECT_TRUE(manager.hasMission());
-
-    auto mission = manager.nextMission();
-    ASSERT_NE(mission, nullptr);
-    EXPECT_EQ(manager.state(), MissionState::RUNNING);
-
-    manager.onNavigationDone();
-    EXPECT_EQ(manager.state(), MissionState::IDLE);
-    EXPECT_FALSE(manager.hasMission());
-}
-
-TEST_F(MissionAdaptersTest, ManagerHandlesPauseResumeCancelAndFailure)
-{
-    MissionManager manager;
-
-    manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0)));
-    manager.pause();
-    EXPECT_EQ(manager.state(), MissionState::PAUSED);
-
-    manager.resume();
-    EXPECT_EQ(manager.state(), MissionState::QUEUED);
-
-    manager.cancel();
-    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
-    EXPECT_FALSE(manager.hasMission());
-
-    manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0)));
-    manager.nextMission();
-    manager.onNavigationFailed();
-    EXPECT_EQ(manager.state(), MissionState::FAILED);
-    EXPECT_FALSE(manager.hasMission());
-}
-
-TEST_F(MissionAdaptersTest, NavigationResultIsIgnoredOutsideRunningState)
-{
-    MissionManager manager;
-
-    manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0)));
-    manager.emergency();
-    manager.onNavigationDone();
-    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
-
-    manager.onNavigationFailed();
-    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
-
-    manager.clearEmergency();
-    manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0)));
-    manager.cancel();
-    manager.onNavigationDone();
-    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
-}
-
-TEST_F(MissionAdaptersTest, EmergencyClearsAndBlocksNewMissionsUntilCleared)
-{
-    MissionManager manager;
-
-    manager.submit(goal_adapter.convert(makeGoal(1.0, 1.0)));
-    manager.emergency();
-    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
-    EXPECT_FALSE(manager.hasMission());
-
-    manager.submit(goal_adapter.convert(makeGoal(2.0, 2.0)));
-    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
-    EXPECT_FALSE(manager.hasMission());
-
-    manager.clearEmergency();
-    manager.submit(goal_adapter.convert(makeGoal(3.0, 3.0)));
-    EXPECT_EQ(manager.state(), MissionState::QUEUED);
-    EXPECT_TRUE(manager.hasMission());
-}
-
-TEST_F(MissionAdaptersTest, EventProcessorProcessesGoalAndEmergency)
-{
-    MissionManager manager;
-    EventProcessor processor(manager);
-
-    processor.start();
-    processor.goalEvent(makeGoal(5.0, 6.0));
-    EXPECT_TRUE(waitForState(manager, MissionState::QUEUED, std::chrono::milliseconds(250)));
-
-    processor.emergencyEvent();
-    EXPECT_TRUE(waitForState(manager, MissionState::EMERGENCY, std::chrono::milliseconds(250)));
-
-    processor.goalEvent(makeGoal(7.0, 8.0));
-    std::this_thread::sleep_for(std::chrono::milliseconds(50));
-    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
-
-    processor.clearEmergencyEvent();
-    EXPECT_TRUE(waitForState(manager, MissionState::CLEAR_EMERGENCY, std::chrono::milliseconds(250)));
-
-    processor.stop();
-}
-
-TEST_F(MissionAdaptersTest, MissionExecutorDispatchesEachMissionOnce)
-{
-    MissionManager manager;
-    MissionExecutor executor(manager);
-    std::atomic<int> callback_count{0};
-
-    executor.setMissionCallback(
-        [&callback_count](const std::shared_ptr<Mission>& mission)
-        {
-            ASSERT_NE(mission, nullptr);
-            ++callback_count;
-        });
-
-    manager.submit(goal_adapter.convert(makeGoal(10.0, 10.0)));
-    executor.start();
-
-    const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(250);
-    while (callback_count.load() < 1 && std::chrono::steady_clock::now() < deadline)
-        std::this_thread::sleep_for(std::chrono::milliseconds(5));
-
-    manager.onNavigationDone();
-    manager.submit(goal_adapter.convert(makeGoal(11.0, 11.0)));
-
-    const auto second_deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(250);
-    while (callback_count.load() < 2 && std::chrono::steady_clock::now() < second_deadline)
-        std::this_thread::sleep_for(std::chrono::milliseconds(5));
-
-    executor.stop();
-
-    EXPECT_EQ(callback_count.load(), 2);
-}
-
-}  // namespace
-
-int main(int argc, char** argv)
-{
-    testing::InitGoogleTest(&argc, argv);
-    return RUN_ALL_TESTS();
-}

test/mission_lifecycle_test.cpp

@@ -0,0 +1,356 @@
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <thread>
+
+#include <mission_adapters/mission_adapters.h>
+#include <mission_adapters/plugin_registry.h>
+
+#include "goal_source_adapter.h"
+#include "vda5050_source_adapter.h"
+#include "mission_test_utils.h"
+
+namespace
+{
+using namespace mission_adapters;
+using mission_test::FakeNavigationClient;
+using mission_test::makeGoal;
+using mission_test::makeMission;
+using mission_test::waitForState;
+
+/// Chờ tới khi predicate đúng hoặc hết hạn. Trả giá trị cuối cùng của predicate.
+template <typename Predicate>
+bool waitFor(Predicate predicate, std::chrono::milliseconds timeout)
+{
+    const auto deadline = std::chrono::steady_clock::now() + timeout;
+    while (std::chrono::steady_clock::now() < deadline)
+    {
+        if (predicate())
+            return true;
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    }
+
+    return predicate();
+}
+
+constexpr std::chrono::milliseconds kWaitTimeout{500};
+
+class MissionLifecycleTest : public ::testing::Test
+{
+protected:
+    void SetUp() override
+    {
+        // Đăng ký thẳng thay vì nạp .so: test này kiểm luồng mission, không kiểm đường Boost.DLL
+        // (việc đó thuộc plugin_registry_test).
+        ASSERT_TRUE(registry.registerAdapter(std::make_shared<mission_plugins::GoalSourceAdapter>()));
+        ASSERT_TRUE(registry.registerAdapter(std::make_shared<mission_plugins::VDA5050SourceAdapter>()));
+    }
+
+    /// Một mission dựng thẳng, đóng gói trong vector để submit().
+    std::vector<std::shared_ptr<Mission>> oneMission(double x, double y)
+    {
+        return {makeMission(x, y)};
+    }
+
+    PluginRegistry registry;
+};
+
+TEST_F(MissionLifecycleTest, EventProcessorProcessesGoalAndEmergency)
+{
+    MissionManager manager;
+    EventProcessor processor(manager, registry);
+
+    processor.start();
+    processor.goalEvent(makeGoal(5.0, 6.0));
+    EXPECT_TRUE(waitForState(manager, MissionState::QUEUED, kWaitTimeout));
+
+    processor.emergencyEvent();
+    EXPECT_TRUE(waitForState(manager, MissionState::EMERGENCY, kWaitTimeout));
+
+    processor.goalEvent(makeGoal(7.0, 8.0));
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+
+    processor.clearEmergencyEvent();
+    EXPECT_TRUE(waitForState(manager, MissionState::CLEAR_EMERGENCY, kWaitTimeout));
+
+    processor.stop();
+}
+
+TEST_F(MissionLifecycleTest, MissionExecutorDispatchesEachMissionOnce)
+{
+    MissionManager manager;
+    MissionExecutor executor(manager);
+    FakeNavigationClient client;
+
+    executor.setNavigationClient(&client);
+    manager.submit(oneMission(10.0, 10.0));
+    executor.start();
+
+    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 1u; }, kWaitTimeout));
+
+    const auto first = client.dispatched().front();
+    ASSERT_TRUE(manager.onNavigationDone(first->id));
+    manager.submit(oneMission(11.0, 11.0));
+
+    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 2u; }, kWaitTimeout));
+
+    // Chặng thứ hai chưa xong -> không được giao thêm lần nào nữa.
+    std::this_thread::sleep_for(std::chrono::milliseconds(120));
+    executor.stop();
+
+    EXPECT_EQ(client.dispatchCount(), 2u);
+    EXPECT_TRUE(client.cancelled().empty());
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// A3 — cancel/emergency ở tầng mission phải tới được navigation.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST_F(MissionLifecycleTest, CancelStopsNavigationExactlyOnce)
+{
+    MissionManager manager;
+    MissionExecutor executor(manager);
+    FakeNavigationClient client;
+
+    executor.setNavigationClient(&client);
+    manager.submit(oneMission(1.0, 1.0));
+    executor.start();
+
+    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 1u; }, kWaitTimeout));
+    const MissionId running_id = client.dispatched().front()->id;
+
+    manager.cancel();
+
+    ASSERT_TRUE(waitFor([&client] { return !client.cancelled().empty(); }, kWaitTimeout));
+    std::this_thread::sleep_for(std::chrono::milliseconds(120));
+    executor.stop();
+
+    ASSERT_EQ(client.cancelled().size(), 1u) << "cancelActive must be called exactly once";
+    EXPECT_EQ(client.cancelled().front(), running_id);
+    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
+}
+
+TEST_F(MissionLifecycleTest, PreemptCancelsOldMissionBeforeDispatchingNewOne)
+{
+    MissionManager manager;
+    MissionExecutor executor(manager);
+    FakeNavigationClient client;
+
+    executor.setNavigationClient(&client);
+    manager.submit(oneMission(1.0, 1.0));
+    executor.start();
+
+    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 1u; }, kWaitTimeout));
+    const MissionId old_id = client.dispatched().front()->id;
+
+    // Order mới thay order cũ trong lúc chặng đầu đang chạy.
+    manager.submit(oneMission(2.0, 2.0));
+
+    ASSERT_TRUE(waitFor([&client] { return client.dispatchCount() == 2u; }, kWaitTimeout));
+    executor.stop();
+
+    ASSERT_EQ(client.cancelled().size(), 1u) << "the replaced leg was not told to stop";
+    EXPECT_EQ(client.cancelled().front(), old_id);
+    EXPECT_NE(client.dispatched().back()->id, old_id);
+}
+
+TEST_F(MissionLifecycleTest, RejectedDispatchFailsMissionInsteadOfHanging)
+{
+    MissionManager manager;
+    MissionExecutor executor(manager);
+    FakeNavigationClient client;
+
+    client.setAcceptDispatch(false);
+    executor.setNavigationClient(&client);
+    manager.submit(oneMission(3.0, 3.0));
+    executor.start();
+
+    EXPECT_TRUE(waitForState(manager, MissionState::FAILED, kWaitTimeout))
+        << "navigation rejected it yet the mission is still stuck in RUNNING";
+
+    executor.stop();
+    EXPECT_EQ(client.dispatchCount(), 1u) << "a rejected mission was handed over again on the next "
+                                             "round";
+}
+
+TEST_F(MissionLifecycleTest, OrderProducingNoMissionLeavesQueueUntouched)
+{
+    MissionManager manager;
+    EventProcessor processor(manager, registry);
+
+    processor.start();
+    processor.goalEvent(makeGoal(4.0, 4.0));
+    ASSERT_TRUE(waitForState(manager, MissionState::QUEUED, kWaitTimeout));
+
+    // Order thiếu edge -> convert() rỗng -> không được đụng vào hàng đợi hiện tại (A1).
+    auto invalid_order = mission_test::makeOrder(4);
+    invalid_order.edges.pop_back();
+    processor.orderEvent(invalid_order);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(80));
+    processor.stop();
+
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission()) << "a failed order cleared the queue";
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// End-to-end: một Order đi hết đường qua cả ba thành phần.
+// ─────────────────────────────────────────────────────────────────────────────
+
+/// Dựng bộ ba thành phần đã nối dây sẵn, dùng chung cho các test end-to-end bên dưới.
+struct MissionStack
+{
+    explicit MissionStack(PluginRegistry& registry)
+        : processor(manager, registry)
+        , executor(manager)
+    {
+        executor.setNavigationClient(&client);
+        processor.start();
+        executor.start();
+    }
+
+    ~MissionStack()
+    {
+        executor.stop();
+        processor.stop();
+    }
+
+    MissionManager manager;
+    EventProcessor processor;
+    MissionExecutor executor;
+    FakeNavigationClient client;
+};
+
+TEST_F(MissionLifecycleTest, OrderRunsThreeLegsThenCompletes)
+{
+    MissionStack stack(registry);
+
+    // Order 4 node, action tại node 1 và node 2 -> cắt thành 3 chặng.
+    auto order = mission_test::makeOrder(4);
+    order.nodes[1].actions.push_back(mission_test::makeAction("lift"));
+    order.nodes[2].actions.push_back(mission_test::makeAction("drop"));
+
+    stack.processor.orderEvent(order);
+
+    for (int leg = 0; leg < 3; ++leg)
+    {
+        const size_t expected = static_cast<size_t>(leg) + 1;
+        ASSERT_TRUE(waitFor([&stack, expected] { return stack.client.dispatchCount() == expected; },
+                            kWaitTimeout))
+            << "leg number " << leg << " was not handed over";
+
+        const auto mission = stack.client.dispatched().back();
+        stack.processor.navDoneEvent(mission->id);
+    }
+
+    EXPECT_TRUE(waitForState(stack.manager, MissionState::COMPLETED, kWaitTimeout));
+    EXPECT_EQ(stack.client.dispatchCount(), 3u);
+    EXPECT_TRUE(stack.client.cancelled().empty());
+}
+
+TEST_F(MissionLifecycleTest, CancelMidOrderThenAcceptNewOrder)
+{
+    MissionStack stack(registry);
+
+    auto order = mission_test::makeOrder(4);
+    order.nodes[1].actions.push_back(mission_test::makeAction("lift"));
+    order.nodes[2].actions.push_back(mission_test::makeAction("drop"));
+
+    stack.processor.orderEvent(order);
+    ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 1u; }, kWaitTimeout));
+
+    const MissionId running_id = stack.client.dispatched().front()->id;
+    stack.processor.cancelEvent();
+
+    ASSERT_TRUE(waitFor([&stack] { return !stack.client.cancelled().empty(); }, kWaitTimeout));
+    EXPECT_EQ(stack.client.cancelled().front(), running_id);
+    EXPECT_TRUE(waitForState(stack.manager, MissionState::CANCELLED, kWaitTimeout));
+
+    // Sau khi huỷ vẫn nhận được order mới.
+    stack.processor.orderEvent(mission_test::makeOrder(2, "order_moi"));
+
+    ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 2u; }, kWaitTimeout));
+    EXPECT_EQ(stack.manager.state(), MissionState::RUNNING);
+    EXPECT_NE(stack.client.dispatched().back()->id, running_id);
+}
+
+// A5 end-to-end: huỷ ngay sau khi phát order thì không mission nào được chạy.
+TEST_F(MissionLifecycleTest, OrderThenImmediateCancelDispatchesNothing)
+{
+    MissionStack stack(registry);
+
+    stack.processor.orderEvent(mission_test::makeOrder(3));
+    stack.processor.cancelEvent();
+
+    // Cho cả hai sự kiện chạy xong rồi mới kết luận.
+    ASSERT_TRUE(waitForState(stack.manager, MissionState::CANCELLED, kWaitTimeout));
+    std::this_thread::sleep_for(std::chrono::milliseconds(120));
+
+    EXPECT_EQ(stack.client.dispatchCount(), 0u)
+        << "the cancel was processed before the order so the order still runs — the robot drives "
+           "what the user just cancelled";
+    EXPECT_FALSE(stack.manager.hasMission());
+}
+
+TEST_F(MissionLifecycleTest, EmergencyOvertakesLongEventQueue)
+{
+    MissionStack stack(registry);
+
+    // Nhồi hàng đợi bằng nhiều goal rồi mới bấm emergency.
+    for (int i = 0; i < 50; ++i)
+        stack.processor.goalEvent(makeGoal(i, i));
+
+    stack.processor.emergencyEvent();
+
+    EXPECT_TRUE(waitForState(stack.manager, MissionState::EMERGENCY, kWaitTimeout))
+        << "emergency must queue-jump, not wait behind 50 events";
+    EXPECT_FALSE(stack.manager.hasMission());
+}
+
+TEST_F(MissionLifecycleTest, ActionsReachNavigationIntactAndInOrder)
+{
+    MissionStack stack(registry);
+
+    auto order = mission_test::makeOrder(3);
+    order.edges[0].sequenceId = 1;
+    order.edges[0].actions.push_back(mission_test::makeAction("beep"));
+    order.nodes[1].sequenceId = 2;
+    order.nodes[1].actions.push_back(mission_test::makeAction("lift"));
+
+    stack.processor.orderEvent(order);
+    ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 1u; }, kWaitTimeout));
+
+    const auto mission = stack.client.dispatched().front();
+    ASSERT_EQ(mission->actions.size(), 2u) << "the action was lost on the way down to navigation";
+    EXPECT_EQ(mission->actions[0].action.actionId, "beep");
+    EXPECT_EQ(mission->actions[1].action.actionId, "lift");
+    EXPECT_TRUE(mission->has_goal);
+}
+
+TEST_F(MissionLifecycleTest, GoallessMissionReachesNavigationWithItsActions)
+{
+    MissionStack stack(registry);
+
+    // Action ngay tại node xuất phát -> chặng đầu không có quãng đường nào để đi.
+    auto order = mission_test::makeOrder(3);
+    order.nodes[0].actions.push_back(mission_test::makeAction("pick_at_start"));
+
+    stack.processor.orderEvent(order);
+    ASSERT_TRUE(waitFor([&stack] { return stack.client.dispatchCount() == 1u; }, kWaitTimeout));
+
+    const auto mission = stack.client.dispatched().front();
+    EXPECT_FALSE(mission->has_goal);
+    ASSERT_EQ(mission->actions.size(), 1u);
+    EXPECT_EQ(mission->actions.front().action.actionId, "pick_at_start");
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

test/mission_manager_test.cpp

@@ -0,0 +1,405 @@
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <thread>
+#include <vector>
+
+#include <mission_adapters/mission_manager.h>
+
+#include "mission_test_utils.h"
+
+namespace
+{
+using namespace mission_adapters;
+using mission_test::makeMission;
+using mission_test::makeMissions;
+
+TEST(MissionManagerTest, ManagerRunsMissionLifecycle)
+{
+    MissionManager manager;
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 2.0)});
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+
+    auto mission = manager.nextMission();
+    ASSERT_NE(mission, nullptr);
+    EXPECT_NE(mission->id, kInvalidMissionId) << "submit must return a MissionId";
+    EXPECT_EQ(manager.state(), MissionState::RUNNING);
+
+    EXPECT_TRUE(manager.onNavigationDone(mission->id));
+    EXPECT_EQ(manager.state(), MissionState::COMPLETED);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST(MissionManagerTest, ManagerHandlesPauseResumeCancelAndFailure)
+{
+    MissionManager manager;
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 1.0)});
+    manager.pause();
+    EXPECT_EQ(manager.state(), MissionState::PAUSED);
+
+    manager.resume();
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+
+    manager.cancel();
+    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
+    EXPECT_FALSE(manager.hasMission());
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(2.0, 2.0)});
+    const auto mission = manager.nextMission();
+    ASSERT_NE(mission, nullptr);
+    EXPECT_TRUE(manager.onNavigationFailed(mission->id));
+    EXPECT_EQ(manager.state(), MissionState::FAILED);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST(MissionManagerTest, NavigationResultIsIgnoredOutsideRunningState)
+{
+    MissionManager manager;
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 1.0)});
+    const MissionId queued_id = manager.nextMission()->id;
+
+    manager.emergency();
+    EXPECT_FALSE(manager.onNavigationDone(queued_id));
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+
+    EXPECT_FALSE(manager.onNavigationFailed(queued_id));
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+
+    manager.clearEmergency();
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(2.0, 2.0)});
+    manager.cancel();
+    EXPECT_FALSE(manager.onNavigationDone(queued_id));
+    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
+}
+
+TEST(MissionManagerTest, EmergencyClearsAndBlocksNewMissionsUntilCleared)
+{
+    MissionManager manager;
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(1.0, 1.0)});
+    manager.emergency();
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+    EXPECT_FALSE(manager.hasMission());
+
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(2.0, 2.0)});
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+    EXPECT_FALSE(manager.hasMission());
+
+    manager.clearEmergency();
+    manager.submit(std::vector<std::shared_ptr<Mission>>{makeMission(3.0, 3.0)});
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// A1 — submit rỗng không được đụng vào hàng đợi đang chạy.
+//
+// Một order lỗi từ fleet manager (convert() trả rỗng) không phải là lệnh "huỷ mọi thứ". Nếu nó xoá
+// queue thì chặng đang chạy mất chủ: robot vẫn đi tới goal cũ vì không ai bảo navigation dừng, còn
+// mission layer thì kẹt ở RUNNING mà không còn mission nào để hoàn tất.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, EmptySubmitLeavesRunningMissionUntouched)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(2));
+
+    const auto running = manager.nextMission();
+    ASSERT_NE(running, nullptr);
+    ASSERT_EQ(manager.state(), MissionState::RUNNING);
+
+    // Order lỗi -> convert() rỗng -> submit rỗng.
+    manager.submit({});
+
+    EXPECT_EQ(manager.state(), MissionState::RUNNING);
+    EXPECT_TRUE(manager.hasMission()) << "an empty submit wiped the running mission and the queue";
+    EXPECT_EQ(manager.currentMissionId(), running->id) << "the running mission was replaced";
+    EXPECT_EQ(manager.takePendingCancel(), kInvalidMissionId)
+        << "an empty submit must not ask navigation to stop";
+
+    // Chặng thứ hai vẫn phải còn trong hàng đợi.
+    EXPECT_TRUE(manager.onNavigationDone(running->id));
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// A2 — outcome phải mang MissionId.
+//
+// Không có ID thì kết quả của chặng cũ (đến trễ, sau khi order đã bị thay) được ghi nhận cho chặng
+// mới: mission mới "hoàn thành" trong khi robot chưa hề chạy nó.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, StaleOutcomeOfPreemptedMissionIsRejected)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    // Order mới thay order cũ trong lúc chặng đầu đang chạy.
+    manager.submit(makeMissions(1));
+    const auto second = manager.nextMission();
+    ASSERT_NE(second, nullptr);
+    ASSERT_NE(first->id, second->id);
+
+    // Outcome của chặng ĐẦU về muộn, sau khi chặng hai đã bắt đầu.
+    EXPECT_FALSE(manager.onNavigationDone(first->id));
+    EXPECT_EQ(manager.state(), MissionState::RUNNING) << "a late outcome completed the wrong "
+                                                         "mission";
+    EXPECT_EQ(manager.currentMissionId(), second->id);
+
+    // Outcome đúng của chặng hai vẫn được nhận bình thường.
+    EXPECT_TRUE(manager.onNavigationDone(second->id));
+    EXPECT_EQ(manager.state(), MissionState::COMPLETED);
+}
+
+TEST(MissionManagerTest, MissionIdsAreUniqueAndMonotonic)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(3));
+
+    MissionId previous = kInvalidMissionId;
+    for (int i = 0; i < 3; ++i)
+    {
+        const auto mission = manager.nextMission();
+        ASSERT_NE(mission, nullptr) << "leg number " << i;
+        EXPECT_GT(mission->id, previous);
+        previous = mission->id;
+
+        ASSERT_TRUE(manager.onNavigationDone(mission->id));
+    }
+
+    EXPECT_EQ(manager.state(), MissionState::COMPLETED);
+}
+
+TEST(MissionManagerTest, RunningMissionIsDispatchedOnlyOnce)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(2));
+
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    // M6: trong lúc RUNNING không có mission mới nào để giao.
+    EXPECT_EQ(manager.nextMission(), nullptr);
+    EXPECT_EQ(manager.nextMission(), nullptr);
+
+    ASSERT_TRUE(manager.onNavigationDone(first->id));
+    const auto second = manager.nextMission();
+    ASSERT_NE(second, nullptr);
+    EXPECT_NE(second->id, first->id);
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// A3 — mission layer phải có đường bảo navigation dừng.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, CancelRequestsNavigationStopOfRunningMission)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(2));
+    const auto running = manager.nextMission();
+    ASSERT_NE(running, nullptr);
+
+    manager.cancel();
+
+    EXPECT_EQ(manager.takePendingCancel(), running->id);
+    EXPECT_EQ(manager.takePendingCancel(), kInvalidMissionId) << "the stop request was taken twice";
+    EXPECT_EQ(manager.state(), MissionState::CANCELLED);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST(MissionManagerTest, EmergencyRequestsNavigationStopOfRunningMission)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    const auto running = manager.nextMission();
+    ASSERT_NE(running, nullptr);
+
+    manager.emergency();
+
+    EXPECT_EQ(manager.takePendingCancel(), running->id);
+    EXPECT_EQ(manager.state(), MissionState::EMERGENCY);
+}
+
+TEST(MissionManagerTest, PreemptRequestsNavigationStopOfReplacedMission)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    manager.submit(makeMissions(1));
+
+    EXPECT_EQ(manager.takePendingCancel(), first->id)
+        << "a new order replaced the old one without telling navigation to stop the old leg";
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+}
+
+TEST(MissionManagerTest, SubmitWhilePausedKeepsRobotStopped)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    manager.pause();
+
+    manager.submit(makeMissions(1));
+
+    EXPECT_EQ(manager.state(), MissionState::PAUSED) << "a new order started running on its own "
+                                                        "while PAUSED";
+    EXPECT_EQ(manager.nextMission(), nullptr);
+
+    manager.resume();
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_NE(manager.nextMission(), nullptr);
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// M8 — một chặng hỏng thì phần còn lại của hàng đợi đi đâu.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, FailureClearsRemainingQueueByDefault)
+{
+    MissionManager manager;   // clear_queue_on_failure mặc định = true
+
+    manager.submit(makeMissions(3));
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    ASSERT_TRUE(manager.onNavigationFailed(first->id));
+
+    // Tuyến tuần tự: không tới được chặng 1 thì chặng 2 nằm sau nó cũng không còn ý nghĩa.
+    EXPECT_EQ(manager.state(), MissionState::FAILED);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST(MissionManagerTest, FailureKeepsQueueWhenConfigured)
+{
+    MissionConfig config;
+    config.clear_queue_on_failure = false;   // các mission trong hàng đợi độc lập với nhau
+    MissionManager manager(config);
+
+    manager.submit(makeMissions(3));
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    ASSERT_TRUE(manager.onNavigationFailed(first->id));
+
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+
+    const auto second = manager.nextMission();
+    ASSERT_NE(second, nullptr);
+    EXPECT_NE(second->id, first->id);
+}
+
+TEST(MissionManagerTest, LastMissionFailureEndsInFailedEvenWhenQueueKept)
+{
+    MissionConfig config;
+    config.clear_queue_on_failure = false;
+    MissionManager manager(config);
+
+    manager.submit(makeMissions(1));
+    const auto only = manager.nextMission();
+    ASSERT_NE(only, nullptr);
+
+    ASSERT_TRUE(manager.onNavigationFailed(only->id));
+    EXPECT_EQ(manager.state(), MissionState::FAILED);
+}
+
+// ─────────────────────────────────────────────────────────────────────────────
+// M7 — mission_timeout là lưới cuối khi navigation không bao giờ báo kết quả về.
+// ─────────────────────────────────────────────────────────────────────────────
+
+TEST(MissionManagerTest, TimeoutDisabledByDefaultLetsMissionRunOn)
+{
+    MissionManager manager;
+
+    manager.submit(makeMissions(1));
+    ASSERT_NE(manager.nextMission(), nullptr);
+
+    // waitForWork phải chặn (không có việc); wakeUp là đường ra duy nhất.
+    std::thread waker([&manager] {
+        std::this_thread::sleep_for(std::chrono::milliseconds(80));
+        manager.wakeUp();
+    });
+
+    EXPECT_FALSE(manager.waitForWork());
+    waker.join();
+
+    EXPECT_EQ(manager.state(), MissionState::RUNNING) << "the mission was cancelled although the "
+                                                         "timeout is off";
+}
+
+TEST(MissionManagerTest, TimeoutFailsMissionAndRequestsNavigationStop)
+{
+    MissionConfig config;
+    config.mission_timeout = 0.15;   // [s]
+    MissionManager manager(config);
+
+    manager.submit(makeMissions(1));
+    const auto running = manager.nextMission();
+    ASSERT_NE(running, nullptr);
+
+    const auto started = std::chrono::steady_clock::now();
+
+    // waitForWork tự thức dậy đúng lúc hết hạn và biến quá hạn thành việc cần làm.
+    ASSERT_TRUE(manager.waitForWork());
+
+    const double elapsed =
+        std::chrono::duration<double>(std::chrono::steady_clock::now() - started).count();
+    EXPECT_GE(elapsed, 0.15) << "the leg was cancelled before it expired";
+
+    EXPECT_EQ(manager.state(), MissionState::FAILED);
+    EXPECT_EQ(manager.takePendingCancel(), running->id)
+        << "it expired but navigation was not told to stop — the robot keeps driving to the old "
+           "goal";
+}
+
+TEST(MissionManagerTest, TimeoutIsMeasuredPerMissionNotPerQueue)
+{
+    MissionConfig config;
+    config.mission_timeout = 0.2;   // [s]
+    config.clear_queue_on_failure = false;
+    MissionManager manager(config);
+
+    manager.submit(makeMissions(2));
+
+    const auto first = manager.nextMission();
+    ASSERT_NE(first, nullptr);
+
+    // Chặng đầu xong sớm; mốc thời gian phải được gieo lại cho chặng hai.
+    std::this_thread::sleep_for(std::chrono::milliseconds(120));
+    ASSERT_TRUE(manager.onNavigationDone(first->id));
+
+    const auto second = manager.nextMission();
+    ASSERT_NE(second, nullptr);
+
+    // Nếu mốc không được gieo lại, chặng hai sẽ hết hạn gần như tức thì.
+    const auto started = std::chrono::steady_clock::now();
+    ASSERT_TRUE(manager.waitForWork());
+    const double elapsed =
+        std::chrono::duration<double>(std::chrono::steady_clock::now() - started).count();
+
+    EXPECT_GE(elapsed, 0.19) << "the second leg inherited the clock of the first one";
+    EXPECT_EQ(manager.takePendingCancel(), second->id);
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

test/mission_test_utils.h

@@ -0,0 +1,177 @@
+/*********************************************************************
+ *
+ * Helper dùng chung cho test của gói: dựng goal/order giả và chờ trạng thái.
+ *
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_
+#define MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_
+
+#include <chrono>
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <mission_adapters/mission_adapters.h>
+
+namespace mission_test
+{
+
+/**
+ * @brief NavigationClient giả: ghi lại mọi lệnh xuống navigation để test kiểm thứ tự và số lần.
+ *
+ * Không mô phỏng chuyển động — test tự quyết định khi nào outcome quay về.
+ */
+class FakeNavigationClient : public mission_adapters::NavigationClient
+{
+public:
+    bool dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission) override
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        dispatched_.push_back(mission);
+        return accept_dispatch_;
+    }
+
+    void cancelActive(mission_adapters::MissionId id) override
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        cancelled_.push_back(id);
+    }
+
+    /// Ép navigation từ chối mọi mission tiếp theo.
+    void setAcceptDispatch(bool accept)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        accept_dispatch_ = accept;
+    }
+
+    std::vector<std::shared_ptr<const mission_adapters::Mission>> dispatched() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return dispatched_;
+    }
+
+    std::vector<mission_adapters::MissionId> cancelled() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return cancelled_;
+    }
+
+    size_t dispatchCount() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return dispatched_.size();
+    }
+
+private:
+    mutable std::mutex mutex_;
+    std::vector<std::shared_ptr<const mission_adapters::Mission>> dispatched_;
+    std::vector<mission_adapters::MissionId> cancelled_;
+    bool accept_dispatch_ = true;
+};
+
+
+/// @brief Goal hợp lệ: quaternion đã chuẩn hoá (w=1) để qua được validate() của adapter.
+inline robot_geometry_msgs::PoseStamped makeGoal(double x, double y)
+{
+    robot_geometry_msgs::PoseStamped goal;
+    goal.header.frame_id = "map";
+    goal.pose.position.x = x;
+    goal.pose.position.y = y;
+    goal.pose.orientation.w = 1.0;
+    return goal;
+}
+
+/// @brief Mission dựng thẳng, không qua adapter — dùng cho test của MissionManager/Executor.
+inline std::shared_ptr<mission_adapters::Mission> makeMission(double x, double y)
+{
+    auto mission = std::make_shared<mission_adapters::Mission>();
+    mission->type = mission_adapters::MissionType::SIMPLE_GOAL;
+    mission->goal = makeGoal(x, y);
+    return mission;
+}
+
+/// @brief n mission độc lập, goal khác nhau.
+inline std::vector<std::shared_ptr<mission_adapters::Mission>> makeMissions(int count)
+{
+    std::vector<std::shared_ptr<mission_adapters::Mission>> missions;
+    missions.reserve(static_cast<size_t>(count));
+    for (int i = 0; i < count; ++i)
+        missions.push_back(makeMission(i, i));
+
+    return missions;
+}
+
+inline robot_protocol_msgs::Action makeAction(const std::string& id)
+{
+    robot_protocol_msgs::Action action;
+    action.actionId = id;
+    action.actionType = "TEST";
+    return action;
+}
+
+/// @brief Node đã released (base). Order thật của fleet manager luôn có ít nhất phần base.
+inline robot_protocol_msgs::Node makeNode(int sequence_id, bool add_action = false)
+{
+    robot_protocol_msgs::Node node;
+    node.sequenceId = sequence_id;
+    node.nodeId = "node_" + std::to_string(sequence_id);
+    node.released = true;
+    node.nodePosition.x = sequence_id;
+    node.nodePosition.y = sequence_id;
+
+    if (add_action)
+        node.actions.push_back(makeAction("node_action_" + std::to_string(sequence_id)));
+
+    return node;
+}
+
+inline robot_protocol_msgs::Edge makeEdge(int sequence_id, bool add_action = false)
+{
+    robot_protocol_msgs::Edge edge;
+    edge.sequenceId = sequence_id;
+    edge.edgeId = "edge_" + std::to_string(sequence_id);
+    edge.released = true;
+
+    if (add_action)
+        edge.actions.push_back(makeAction("edge_action_" + std::to_string(sequence_id)));
+
+    return edge;
+}
+
+inline robot_protocol_msgs::Order makeOrder(int node_count, const std::string& order_id = "order_1",
+                                           std::uint32_t order_update_id = 0)
+{
+    robot_protocol_msgs::Order order;
+    order.orderId = order_id;
+    order.orderUpdateId = order_update_id;
+
+    for (int i = 0; i < node_count; ++i)
+        order.nodes.push_back(makeNode(i));
+
+    for (int i = 0; i < node_count - 1; ++i)
+        order.edges.push_back(makeEdge(i));
+
+    return order;
+}
+
+inline bool waitForState(mission_adapters::MissionManager& manager,
+                         mission_adapters::MissionState expected,
+                         std::chrono::milliseconds timeout)
+{
+    const auto deadline = std::chrono::steady_clock::now() + timeout;
+    while (std::chrono::steady_clock::now() < deadline)
+    {
+        if (manager.state() == expected)
+            return true;
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    }
+
+    return manager.state() == expected;
+}
+
+}  // namespace mission_test
+
+#endif  // MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_

test/plugin_registry_test.cpp

@@ -0,0 +1,287 @@
+/*********************************************************************
+ *
+ * Kiểm đường nạp plugin thật: YAML -> library_path -> Boost.DLL -> schema.
+ *
+ * Test này cần PNKX_NAV_CORE_CONFIG_DIR trỏ vào test/config của gói:
+ *
+ *   PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/mission_adapters/test/config \
+ *     ./devel/lib/mission_adapters/plugin_registry_test
+ *
+ *********************************************************************/
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <robot/robot.h>
+
+#include <mission_adapters/mission_adapters.h>
+#include <mission_adapters/plugin_registry.h>
+
+#include "mission_test_utils.h"
+
+namespace
+{
+using namespace mission_adapters;
+
+/**
+ * @brief Nguồn mission thứ ba, viết hoàn toàn trong test.
+ *
+ * Sự tồn tại của nó là bài kiểm tra thật cho tính plugin: thêm một loại nguồn mới mà không sửa file
+ * nào trong `src/` của gói.
+ */
+class DummySourceAdapter : public MissionSourceAdapter
+{
+public:
+    static constexpr const char* kSchema = "test.dummy";
+
+    bool configure(const std::string& name, robot::NodeHandle& nh) override
+    {
+        (void)nh;
+        name_ = name;
+        return true;
+    }
+
+    std::string schema() const override { return kSchema; }
+
+    bool validate(const MissionRequest& request, std::string& reason) const override
+    {
+        if (request.raw_payload.empty())
+        {
+            reason = "raw_payload is empty";
+            return false;
+        }
+        return true;
+    }
+
+    ConversionResult convert(const MissionRequest& request) override
+    {
+        (void)request;
+        auto mission = std::make_shared<Mission>();
+        mission->type = MissionType::SIMPLE_GOAL;
+
+        ConversionResult result;
+        result.missions.push_back(mission);
+        return result;
+    }
+
+private:
+    std::string name_;
+};
+
+class PluginRegistryTest : public ::testing::Test
+{
+protected:
+    robot::NodeHandle nh;
+};
+
+// ── Đường nạp bình thường ────────────────────────────────────────────────────────────────────────
+
+TEST_F(PluginRegistryTest, LoadsBothSourcesFromConfig)
+{
+    PluginRegistry registry;
+
+    ASSERT_TRUE(registry.loadFromConfig(nh))
+        << "could not load the plugin — check PNKX_NAV_CORE_CONFIG_DIR and devel/lib";
+
+    EXPECT_EQ(registry.size(), 2u);
+    EXPECT_NE(registry.find(schema::kPoseStamped), nullptr);
+    EXPECT_NE(registry.find(schema::kVda5050Order), nullptr);
+    EXPECT_EQ(registry.find("does.not.exist"), nullptr);
+}
+
+TEST_F(PluginRegistryTest, LoadedAdapterConvertsRealPayload)
+{
+    PluginRegistry registry;
+    ASSERT_TRUE(registry.loadFromConfig(nh));
+
+    auto* adapter = registry.find(schema::kPoseStamped);
+    ASSERT_NE(adapter, nullptr);
+
+    const auto request = MissionRequest::fromPose(mission_test::makeGoal(3.0, 4.0));
+
+    std::string reason;
+    ASSERT_TRUE(adapter->validate(request, reason)) << reason;
+
+    const auto result = adapter->convert(request);
+    ASSERT_EQ(result.missions.size(), 1u);
+    EXPECT_DOUBLE_EQ(result.missions.front()->goal.pose.position.x, 3.0);
+}
+
+// ── Các cách hỏng, đều phải báo lỗi rõ ràng chứ không crash ──────────────────────────────────────
+
+TEST_F(PluginRegistryTest, MissingLibraryPathKeyFailsCleanly)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_missing_library_path"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+TEST_F(PluginRegistryTest, MissingLibraryFileFailsCleanly)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_missing_library_file"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+TEST_F(PluginRegistryTest, WrongSymbolNameFailsCleanly)
+{
+    PluginRegistry registry;
+
+    // Thư viện có thật, symbol thì không: import_alias ném system_error, registry phải nuốt và báo.
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_wrong_symbol"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+TEST_F(PluginRegistryTest, DuplicateSchemaIsRejected)
+{
+    PluginRegistry registry;
+
+    // Hai instance cùng khai schema "geometry.pose_stamped": cái thứ hai bị từ chối thay vì ghi đè
+    // im lặng, vì định tuyến khi đó sẽ phụ thuộc thứ tự nạp.
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_duplicate_schema"));
+    EXPECT_EQ(registry.size(), 1u);
+}
+
+TEST_F(PluginRegistryTest, EntryWithoutTypeFailsCleanly)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_entry_without_type"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+TEST_F(PluginRegistryTest, MissingSourceListFailsCleanly)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.loadFromConfig(nh, "registry_test_empty"));
+    EXPECT_EQ(registry.size(), 0u);
+}
+
+// ── Đăng ký trực tiếp (nguồn biên dịch thẳng vào host, hoặc test) ───────────────────────────────
+
+TEST_F(PluginRegistryTest, RejectsNullAndDuplicateManualRegistration)
+{
+    PluginRegistry registry;
+
+    EXPECT_FALSE(registry.registerAdapter(nullptr));
+
+    auto first = std::make_shared<DummySourceAdapter>();
+    EXPECT_TRUE(registry.registerAdapter(first));
+
+    auto second = std::make_shared<DummySourceAdapter>();
+    EXPECT_FALSE(registry.registerAdapter(second)) << "two adapters with the same schema were both "
+                                                      "accepted";
+
+    EXPECT_EQ(registry.size(), 1u);
+}
+
+/**
+ * Nguồn thứ ba đi hết đường: đăng ký -> EventProcessor định tuyến theo schema -> mission vào hàng
+ * đợi. Không có dòng nào trong `src/` của gói biết tới DummySourceAdapter.
+ */
+TEST_F(PluginRegistryTest, ThirdPartyAdapterFlowsThroughCoreUnchanged)
+{
+    PluginRegistry registry;
+    ASSERT_TRUE(registry.registerAdapter(std::make_shared<DummySourceAdapter>()));
+
+    MissionManager manager;
+    EventProcessor processor(manager, registry);
+    processor.start();
+
+    processor.submitRequest(
+        MissionRequest::fromRaw(DummySourceAdapter::kSchema, "{\"job\":\"pick\"}"));
+
+    EXPECT_TRUE(mission_test::waitForState(manager, MissionState::QUEUED,
+                                           std::chrono::milliseconds(500)));
+
+    // Payload không hợp lệ -> validate() chặn -> hàng đợi không đổi (A1).
+    processor.submitRequest(MissionRequest::fromRaw(DummySourceAdapter::kSchema, ""));
+    std::this_thread::sleep_for(std::chrono::milliseconds(80));
+
+    EXPECT_EQ(manager.state(), MissionState::QUEUED);
+    EXPECT_TRUE(manager.hasMission());
+
+    processor.stop();
+}
+
+/**
+ * @brief Plugin hỏng: sinh mission không goal và cũng không action.
+ *
+ * Core phải chặn, vì chặng như vậy không có việc gì để làm và sẽ không bao giờ báo kết quả về —
+ * mission layer kẹt RUNNING vĩnh viễn.
+ */
+class EmptyMissionAdapter : public MissionSourceAdapter
+{
+public:
+    static constexpr const char* kSchema = "test.empty_mission";
+
+    bool configure(const std::string&, robot::NodeHandle&) override { return true; }
+    std::string schema() const override { return kSchema; }
+    bool validate(const MissionRequest&, std::string&) const override { return true; }
+
+    ConversionResult convert(const MissionRequest&) override
+    {
+        auto mission = std::make_shared<Mission>();
+        mission->has_goal = false;   // và actions rỗng
+
+        ConversionResult result;
+        result.missions.push_back(mission);
+        return result;
+    }
+};
+
+TEST_F(PluginRegistryTest, MissionWithoutGoalAndWithoutActionIsRejected)
+{
+    PluginRegistry registry;
+    ASSERT_TRUE(registry.registerAdapter(std::make_shared<EmptyMissionAdapter>()));
+
+    MissionManager manager;
+    EventProcessor processor(manager, registry);
+
+    processor.start();
+    processor.submitRequest(MissionRequest::fromRaw(EmptyMissionAdapter::kSchema, "payload"));
+    std::this_thread::sleep_for(std::chrono::milliseconds(80));
+    processor.stop();
+
+    EXPECT_EQ(manager.state(), MissionState::IDLE);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+TEST_F(PluginRegistryTest, UnknownSchemaIsIgnoredNotCrashing)
+{
+    PluginRegistry registry;
+    MissionManager manager;
+    EventProcessor processor(manager, registry);
+
+    processor.start();
+    processor.submitRequest(MissionRequest::fromRaw("nobody.handles.this", "payload"));
+    std::this_thread::sleep_for(std::chrono::milliseconds(80));
+    processor.stop();
+
+    EXPECT_EQ(manager.state(), MissionState::IDLE);
+    EXPECT_FALSE(manager.hasMission());
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+    // Test tự trỏ vào cây config và thư mục .so của chính nó, để chạy được cả qua ctest lẫn khi gọi
+    // thẳng binary (đúng cách test_costmap đang làm). overwrite = 0 nên biến môi trường do người
+    // chạy đặt vẫn thắng.
+#ifdef MISSION_ADAPTERS_TEST_CONFIG_DIR
+    setenv("PNKX_NAV_CORE_CONFIG_DIR", MISSION_ADAPTERS_TEST_CONFIG_DIR, 0);
+#endif
+#ifdef MISSION_ADAPTERS_TEST_LIBRARY_DIR
+    setenv("PNKX_NAV_CORE_LIBRARY_PATH", MISSION_ADAPTERS_TEST_LIBRARY_DIR, 0);
+#endif
+
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}