# Thảo luận về Kiến trúc Navigation cho AMR

## Mục tiêu dự án

Nghiên cứu và phát triển hệ thống navigation (di chuyển bám quỹ đạo) cho robot AMR dạng hai bánh vi sai, **không sử dụng ROS**, viết thuần C++ với CMake trên Linux.

## Yêu cầu chức năng

Từ `readme.md`:

- Có khả năng thay đổi mô hình kinematics (dạng hai bánh vi sai, dạng steering bicycle, ...)
- Có chức năng di chuyển từ điểm khởi đầu đến điểm đích
- Có chức năng xoay tròn
- Có chức năng đi thẳng
- Có chức năng thay đổi vận tốc

## Sơ đồ kiến trúc hệ thống

Kiến trúc được thiết kế theo mô hình layered, tách biệt các concerns và cho phép mở rộng dễ dàng:

```mermaid
flowchart TB
  %% ========== API LAYER ==========
  subgraph API["🌐 API Layer"]
    direction TB
    CAPI["<b>📦 C API</b><br/>━━━━━━━━━━━━━━━━<br/>🔌 nav_c_api<br/>💻 P/Invoke cho .NET/C#<br/>📝 Wrapper Functions<br/>🔗 Interop Layer"]
    style API fill:#E3F2FD,stroke:#1976D2,stroke-width:4px,color:#000
    style CAPI fill:#BBDEFB,stroke:#1976D2,stroke-width:3px,font-size:14px
  end

  %% ========== USER CONTROLLER LAYER ==========
  subgraph UserController["🎮 User Controller Layer"]
    direction TB
    UserCtrl["<b>🔌 User Controller Plugin</b><br/>━━━━━━━━━━━━━━━━<br/>📚 Dynamic Loader<br/>⚙️ boost::dll<br/>🎯 Custom Behavior<br/>🔄 Runtime Loading"]
    style UserController fill:#F3E5F5,stroke:#7B1FA2,stroke-width:4px,color:#000
    style UserCtrl fill:#E1BEE7,stroke:#7B1FA2,stroke-width:3px,font-size:14px
  end

  %% ========== INTERFACE LAYER ==========
  subgraph Interface["🔌 Interface Layer"]
    direction TB
    BaseNav["<b>📋 BaseNavigation</b><br/>━━━━━━━━━━━━━━━━<br/>🏗️ move_base_core::BaseNavigation<br/>🎯 Abstract Interface<br/>━━━━━━━━━━━━━━━━<br/>📍 moveTo, dockTo<br/>🔄 rotateTo, moveStraightTo<br/>⏸️ pause, resume, cancel<br/>📊 getRobotPose"]
    style Interface fill:#FFF3E0,stroke:#E65100,stroke-width:4px,color:#000
    style BaseNav fill:#FFE0B2,stroke:#E65100,stroke-width:3px,font-size:14px
  end

  %% ========== MOVE BASE CORE ==========
  subgraph MoveBaseCore["⚙️ Move Base Core"]
    direction TB
    MoveBase["<b>🚀 MoveBase</b><br/>━━━━━━━━━━━━━━━━<br/>📦 move_base::MoveBase<br/>🔄 State Machine<br/>━━━━━━━━━━━━━━━━<br/>📊 PLANNING<br/>🎮 CONTROLLING<br/>🧹 CLEARING<br/>━━━━━━━━━━━━━━━━<br/>🎛️ Control Loop<br/>🔄 executeCycle"]
    style MoveBaseCore fill:#E8F5E9,stroke:#2E7D32,stroke-width:4px,color:#000
    style MoveBase fill:#C8E6C9,stroke:#2E7D32,stroke-width:3px,font-size:14px
  end

  %% ========== PLANNING LAYER ==========
  subgraph Planning["🗺️ Planning Layer - Plugin System"]
    direction LR
    GP["<b>🌍 Global Planner</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_core::BaseGlobalPlanner<br/>🔌 Dynamic Plugin<br/>━━━━━━━━━━━━━━━━<br/>🎯 A*, D*, Hybrid A*<br/>📐 Long-range Planning<br/>🗺️ Global Path"]
    LP["<b>📍 Local Planner</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_core::BaseLocalPlanner<br/>🔌 Dynamic Plugin<br/>━━━━━━━━━━━━━━━━<br/>🎯 DWA, TEB, MKT<br/>🚗 Velocity Commands<br/>🛡️ Obstacle Avoidance"]
    RB["<b>🔄 Recovery Behaviors</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_core::RecoveryBehavior<br/>🔌 Dynamic Plugin<br/>━━━━━━━━━━━━━━━━<br/>🧹 Clear Costmap<br/>🔄 Rotate Recovery<br/>🚨 Emergency Stop"]
    style Planning fill:#E1F5FE,stroke:#0277BD,stroke-width:4px,color:#000
    style GP fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px
    style LP fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px
    style RB fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px
  end

  %% ========== COSTMAP LAYER ==========
  subgraph Costmap["🗺️ Costmap Layer"]
    direction LR
    GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
    style Costmap fill:#F1F8E9,stroke:#558B2F,stroke-width:4px,color:#000
    style GC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
    style LC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
  end

  %% ========== ALGORITHMS LAYER ==========
  subgraph Algorithms["🧮 Algorithms Layer"]
    direction LR
    MKTAlgo["<b>🚗 MKT Algorithm</b><br/>━━━━━━━━━━━━━━━━<br/>⚙️ Diff Drive Kinematics<br/>🚲 Bicycle Kinematics<br/>📐 Trajectory Generation"]
    ScoreAlgo["<b>📊 Score Algorithm</b><br/>━━━━━━━━━━━━━━━━<br/>📈 Trajectory Scoring<br/>✅ Goal Checking<br/>🎯 Path Evaluation"]
    KalmanAlgo["<b>🔍 Kalman Filter</b><br/>━━━━━━━━━━━━━━━━<br/>📊 State Estimation<br/>🔮 Sensor Fusion<br/>📉 Noise Filtering"]
    style Algorithms fill:#FCE4EC,stroke:#C2185B,stroke-width:4px,color:#000
    style MKTAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px
    style ScoreAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px
    style KalmanAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px
  end

  %% ========== DATA SOURCES ==========
  subgraph DataSources["📡 Data Sources"]
    direction TB
    Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
    Loc["<b>🌍 Localization</b><br/>━━━━━━━━━━━━━━━━<br/>📍 Pnkx Loc<br/>🗺️ Global Pose<br/>🔄 Pose Updates"]
    TF["<b>🔄 Transform System</b><br/>━━━━━━━━━━━━━━━━<br/>📐 tf3::BufferCore<br/>🌐 Coordinate Frames<br/>⏱️ Time Synchronization"]
    Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
    Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 robot_sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
    Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 robot_nav_msgs::OccupancyGrid<br/>🏗️ Static Map<br/>📐 Map Metadata"]
    style DataSources fill:#FFF9C4,stroke:#F57F17,stroke-width:4px,color:#000
    style Goal fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
    style Loc fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
    style TF fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
    style Odom fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
    style Laser fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
    style Map fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
  end

  %% ========== CONTROL LOOP ==========
  subgraph ControlLoop["🔄 Control Loop"]
    direction LR
    CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 robot_geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
    BaseCtrl["<b>🎮 Base Controller</b><br/>━━━━━━━━━━━━━━━━<br/>🚗 diff_driver_controller<br/>🚲 steer_drive_controller<br/>━━━━━━━━━━━━━━━━<br/>⚙️ Kinematics<br/>🔧 Hardware Interface"]
    style ControlLoop fill:#FFEBEE,stroke:#C62828,stroke-width:4px,color:#000
    style CmdVel fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px
    style BaseCtrl fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px
  end

  %% ========== CONNECTIONS ==========
  %% API to User Controller
  CAPI -->|"🔗 P/Invoke"| UserCtrl

  %% User Controller to Interface
  UserCtrl -->|"🎯 Uses"| BaseNav

  %% Interface to MoveBase
  BaseNav -->|"⚙️ Implements"| MoveBase

  %% MoveBase to Planning
  MoveBase -->|"🎛️ Manages"| GP
  MoveBase -->|"🎛️ Manages"| LP
  MoveBase -->|"🎛️ Manages"| RB

  %% MoveBase to Costmap
  MoveBase -->|"🎛️ Manages"| GC
  MoveBase -->|"🎛️ Manages"| LC

  %% Data Sources to Components
  Goal -->|"📥 Input"| UserCtrl
  Goal -->|"📥 Input"| BaseNav
  Loc -->|"📍 Pose"| MoveBase
  TF -->|"🔄 Transforms"| MoveBase
  Laser -->|"📡 Scan Data"| LC
  Map -->|"🗺️ Static Map"| GC
  Odom -->|"⚡ Velocity"| LP
  Odom -->|"📍 Pose"| MoveBase

  %% Planning Flow
  GC -->|"🗺️ Costmap"| GP
  GP -->|"🛤️ Global Path"| LP
  LC -->|"🗺️ Costmap"| LP
  LC -->|"🗺️ Costmap"| RB
  GC -->|"🗺️ Costmap"| RB

  %% Algorithm Integration
  MKTAlgo -->|"🚗 Kinematics"| LP
  ScoreAlgo -->|"📊 Scoring"| LP
  KalmanAlgo -.->|"🔍 Filtering"| Loc

  %% Control Flow
  LP -->|"⚡ Velocity Cmd"| CmdVel
  CmdVel -->|"▶️ Execute"| BaseCtrl
  BaseCtrl -->|"📊 Feedback"| Odom

  %% Styling Classes
  classDef implemented fill:#C8E6C9,stroke:#2E7D32,stroke-width:4px,color:#000
  classDef partial fill:#FFF9C4,stroke:#F57F17,stroke-width:3px,color:#000
  classDef todo fill:#FFCDD2,stroke:#C62828,stroke-width:3px,color:#000

  class BaseNav,MoveBase,GP,LP,RB,GC,LC,CAPI,MKTAlgo,ScoreAlgo,KalmanAlgo,UserCtrl implemented
  class BaseCtrl,Loc,Odom,Map,Laser,TF partial
```

## Đề xuất giải pháp kiến trúc

### 1. Xác định phạm vi & yêu cầu vận hành

Cần làm rõ:
- **Kịch bản sử dụng chính**: go-to-goal trong môi trường indoor/outdoor? Tốc độ di chuyển? Độ chính xác yêu cầu?
- **Cảm biến có sẵn**: encoder, IMU, lidar, camera? Dữ liệu nào đã có?
- **Yêu cầu real-time**: chu kỳ điều khiển (control loop frequency), độ trễ tối đa cho phép?

### 2. Kiến trúc tổng thể

**Các layer chính:**

1. **API Layer** ✅
   - C API (`nav_c_api`) cho P/Invoke với .NET/C#
   - Wrapper functions cho tất cả BaseNavigation methods

2. **User Controller Layer** ✅
   - **User Controller Plugin**: Dynamically allocated từ loader controller plugin
   - Sử dụng `boost::dll` để load plugins động
   - Cho phép user định nghĩa controller riêng để điều khiển navigation behavior
   - Kết nối với `BaseNavigation` interface

3. **Interface Layer** ✅
   - `move_base_core::BaseNavigation`: Abstract interface cho navigation
   - Định nghĩa các operations: moveTo, dockTo, rotateTo, moveStraightTo, pause, resume, cancel
   - Là lớp trung gian định nghĩa các phương thức và thuộc tính liên quan đến navigation

4. **Move Base Core** ✅
   - `move_base::MoveBase`: Core implementation của BaseNavigation
   - Quản lý state machine (PLANNING, CONTROLLING, CLEARING)
   - Điều phối global/local planner và recovery behaviors
   - Quản lý costmaps (global và local)
   - Thực thi control loop (executeCycle)

4. **Planning Layer** ✅
   - `robot_nav_core::BaseGlobalPlanner`: Interface cho global planners
   - **Global Planner Implementations**:
     - `custom_planner` - Custom path planner với curve support và merge path calculation
     - `dock_planner` - Docking-specific planner với dock calculation utilities
     - `two_points_planner` - Simple planner cho two-point navigation
   - `robot_nav_core::BaseLocalPlanner`: Interface cho local planners
   - **Local Planner Implementation**:
     - `pnkx_local_planner` - Main local planner với multiple behaviors:
       - Position planning (MKT algorithm)
       - Docking planning
       - Go straight planning
       - Rotate planning
   - `robot_nav_core::RecoveryBehavior`: Interface cho recovery behaviors
   - Plugin system sử dụng `boost::dll` và `robot::PluginLoaderHelper` để dynamic loading

5. **Costmap Layer** ✅
   - `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap wrapper
   - `robot_costmap_2d::LayeredCostmap`: Layered costmap system
   - **Costmap Layers** (8 implementations):
     - `StaticLayer` - Static map từ OccupancyGrid
     - `ObstacleLayer` - Dynamic obstacles từ sensors
     - `InflationLayer` - Cost inflation cho robot footprint
     - `VoxelLayer` - 3D voxel grid cho obstacles
     - `CriticalLayer` - Critical zones marking
     - `DirectionalLayer` - Directional cost preferences
     - `PreferredLayer` - Preferred path zones
     - `UnpreferredLayer` - Unpreferred zones
   - Frame management: map (global), odom (local)
   - Thread-safe access với mutex
   - Observation buffer cho sensor data

6. **Algorithms Layer** ✅
   - **MKT Algorithm** (`mkt_algorithm`):
     - Differential drive kinematics:
       - `diff_go_straight` - Go straight behavior
       - `diff_predictive_trajectory` - Predictive trajectory following
       - `diff_rotate_to_goal` - Rotate to goal
     - Bicycle model kinematics:
       - `bicycle_go_straight` - Go straight với bicycle model
       - `bicycle_pure_pursuit` - Pure pursuit control
       - `bicycle_rotate_to_goal` - Rotate behavior
   - **MKT Plugins** (`mkt_plugins`):
     - `goal_checker` - Goal checking algorithms
     - `simple_goal_checker` - Simple goal checking
     - `standard_traj_generator` - Standard trajectory generation
     - `limited_accel_generator` - Acceleration-limited trajectory generation
     - `kinematic_parameters` - Kinematics parameter management
     - `xy_theta_iterator` - Trajectory iteration utilities
     - `velocity_iterator` - Velocity space iteration
     - `one_d_velocity_iterator` - 1D velocity iteration
   - **Score Algorithm** (`score_algorithm`):
     - Trajectory scoring và evaluation
     - Goal checking utilities
     - Trajectory generator interface
   - **Kalman Filter** (`kalman`):
     - State estimation
     - Sensor fusion
     - Noise filtering

7. **Data Sources** ⚠️ (Interface cần định nghĩa)
   - Localization source (Pnkx Loc)
   - Odometry source
   - Sensor transforms (tf3)
   - Map server
   - Laser sensors

8. **Control Layer** ⚠️ (Cần implementation)
   - Base Controller interface (Diff/Steer drive)
   - Velocity command execution

**Định dạng dữ liệu:**
- `robot_geometry_msgs::Pose2D` / `robot_geometry_msgs::PoseStamped` (vị trí + hướng)
- `robot_geometry_msgs::Twist` (vận tốc linear/angular)
- `std::vector<robot_geometry_msgs::PoseStamped>` (đường đi)
- `robot_costmap_2d::Costmap2D` (bản đồ chi phí)

### 3. Thiết kế từng module (interface level)

**Các interface đã có:** ✅

- `move_base_core::BaseNavigation` ✅
  - `initialize(TFListenerPtr)` - Khởi tạo với TF listener
  - `moveTo(goal, xy_tol, yaw_tol)` - Di chuyển đến goal
  - `dockTo(marker, goal, ...)` - Docking đến marker
  - `rotateTo(goal, yaw_tol)` - Xoay tại chỗ
  - `moveStraightTo(goal, xy_tol)` - Đi thẳng
  - `pause()`, `resume()`, `cancel()` - Điều khiển trạng thái
  - `getRobotPose(...)` - Lấy vị trí robot

- `robot_nav_core::BaseGlobalPlanner` ✅
  - `makePlan(start, goal, plan)` - Tạo global path
  - `initialize(name, costmap_robot)` - Khởi tạo với costmap
  - **Implementations**: `custom_planner`, `dock_planner`, `two_points_planner`

- `robot_nav_core::BaseLocalPlanner` ✅
  - `computeVelocityCommands(odom, cmd_vel)` - Tính toán velocity command
  - `setPlan(plan)` - Set global path để follow
  - `isGoalReached()` - Kiểm tra đã đến goal chưa
  - `swapPlanner(name)` - Thay đổi planner động (position, docking, go_straight, rotate)
  - `setTwistLinear/Angular(...)` - Set velocity limits
  - `getTwistLinear/Angular(...)` - Get velocity limits
  - **Implementation**: `pnkx_local_planner` với multiple behaviors

- `robot_nav_core::RecoveryBehavior` ✅
  - `runBehavior()` - Thực thi recovery behavior
  - `initialize(name, tf, global_costmap, local_costmap)` - Khởi tạo

- `robot_costmap_2d::Costmap2DROBOT` ✅
  - Wrapper cho costmap với robot footprint
  - Thread-safe access với mutex

**Các interface cần bổ sung:** ⚠️

- `ILocalizationSource` ⚠️
  - `getCurrentPose()` - Lấy vị trí hiện tại từ localization
  - `isAvailable()` - Kiểm tra localization có sẵn không

- `IOdometrySource` ⚠️
  - `getCurrentPose()` - Lấy vị trí từ odometry
  - `getCurrentVelocity()` - Lấy vận tốc hiện tại
  - `isAvailable()` - Kiểm tra odometry có sẵn không

- `IBaseController` ⚠️
  - `executeVelocity(cmd_vel)` - Thực thi velocity command
  - `stop()` - Dừng robot ngay lập tức
  - `getCurrentVelocity()` - Lấy vận tốc thực tế

- `IMapProvider` ⚠️
  - `getMap()` - Lấy static map
  - `isMapAvailable()` - Kiểm tra map có sẵn không

**Plugin mechanism:** ✅
- Sử dụng `boost::dll` để dynamic loading plugins
- Factory pattern với `boost::function` và `boost::dll::import_alias`
- `robot::PluginLoaderHelper` để tìm library path từ plugin name
- Config file YAML để specify plugin names
- Plugin interfaces: 
  - `robot_nav_core::BaseGlobalPlanner` - Global planners
  - `robot_nav_core::BaseLocalPlanner` - Local planners  
  - `robot_nav_core::RecoveryBehavior` - Recovery behaviors
  - `robot_costmap_2d::Layer` - Costmap layers
- Plugin registration với `BOOST_DLL_ALIAS` macro

### 4. Cơ chế giao tiếp & đồng bộ

**Lựa chọn transport:**
- Shared memory + mutex (cho real-time nhẹ)
- Message queue (ZeroMQ, nanomsg)
- Event loop với callback thread-safe

**Time synchronization:**
- Sử dụng `std::chrono` cho timestamp
- Buffer dữ liệu để xử lý dữ liệu không đồng bộ

**Threading strategy:**
- Mỗi module một thread riêng, hoặc
- Scheduler chung quản lý tất cả

### 5. Chiến lược an toàn & recovery

**Monitoring:**
- Heartbeat mechanism
- Watchdog timer

**Recovery behaviors:**
- Không tìm được đường đi
- Mất localization
- Obstacle chặn đường
- Emergency stop mechanism
- Giới hạn vận tốc theo trạng thái

### 6. Config & Logging/Diagnostics

**Configuration:**
- File YAML/JSON cho:
  - Kinematics parameters
  - Velocity limits
  - Planner parameters
  - Sensor configurations

**Logging & Debugging:**
- Logging framework (spdlog?)
- Telemetry interface
- Visualizer tool (SDL/ImGui) để debug map/path

### 7. Trạng thái triển khai & lộ trình

**Đã hoàn thành:** ✅

1. ✅ **Interface Layer**: 
   - `move_base_core::BaseNavigation` - Abstract interface hoàn chỉnh
   - `move_base_core::common.h` - Common types và utilities

2. ✅ **Implementation Layer**: 
   - `move_base::MoveBase` - Core implementation với state machine
   - `executeCycle()` - Control loop implementation
   - State management (PLANNING, CONTROLLING, CLEARING, PAUSED)
   - Threading với planner thread và control loop

3. ✅ **Planning Layer - Plugin System**: 
   - **Global Planners** (3 implementations):
     - `custom_planner` - Custom path planning với curve support
     - `dock_planner` - Docking-specific planner
     - `two_points_planner` - Simple two-point planner
   - **Local Planners** (1 implementation):
     - `pnkx_local_planner` - Main local planner với:
       - `pnkx_docking_local_planner` - Docking behavior
       - `pnkx_go_straight_local_planner` - Go straight behavior
       - `pnkx_rotate_local_planner` - Rotate behavior
   - **Recovery Behaviors**: Interface đã định nghĩa
   - Plugin loading với `boost::dll` và `robot::PluginLoaderHelper`

4. ✅ **Costmap Layer**: 
   - `robot_costmap_2d::Costmap2DROBOT` - Global và local costmap
   - **Costmap Layers** (8 implementations):
     - `static_layer` - Static map layer
     - `obstacle_layer` - Dynamic obstacles
     - `inflation_layer` - Cost inflation
     - `voxel_layer` - 3D voxel grid
     - `critical_layer` - Critical zones
     - `directional_layer` - Directional costs
     - `preferred_layer` - Preferred paths
     - `unpreferred_layer` - Unpreferred zones
   - Layered costmap system với plugin support

5. ✅ **Algorithms Layer**: 
   - **MKT Algorithm**:
     - Differential drive kinematics (go_straight, rotate, predictive)
     - Bicycle model kinematics (go_straight, rotate, pure pursuit)
   - **MKT Plugins**:
     - `goal_checker` - Goal checking algorithms
     - `standard_traj_generator` - Standard trajectory generation
     - `kinematic_parameters` - Kinematics parameter management
     - `xy_theta_iterator` - Trajectory iteration
     - `limited_accel_generator` - Acceleration-limited trajectories
   - **Score Algorithm**: Trajectory scoring và goal checking
   - **Kalman Filter**: State estimation và filtering

6. ✅ **API Layer**: 
   - `nav_c_api` - C API wrapper cho .NET/C# P/Invoke
   - Wrapper functions cho BaseNavigation methods

7. ✅ **Supporting Libraries**: 
   - `tf3` - Transform system (buffer_core, transforms)
   - `robot_time` - Time management (Time, Duration, Timer, Rate)
   - `robot_cpp` - Core utilities:
     - `robot::init()` - Initialization
     - `robot::NodeHandle` - YAML config loading
     - `robot::PluginLoaderHelper` - Plugin discovery
   - `robot_nav_2d_utils` - 2D navigation utilities (conversions, path_ops, polygons, bounds, tf_help)
   - `robot_nav_2d_msgs` - 2D navigation messages
   - `geometry_msgs` - Geometry message types (29 types)
   - `robot_nav_msgs` - Navigation messages (24 types)
   - `robot_sensor_msgs` - Sensor messages (30 types)
   - `laser_geometry` - Laser scan geometry utilities
   - `voxel_grid` - Voxel grid implementation
   - `data_convert` - Data conversion utilities
   - `nav_grid` - Navigation grid utilities

**Đang triển khai / Cần bổ sung:** ⚠️

1. ⚠️ **Data Sources Interfaces**: 
   - `ILocalizationSource` interface - Cần định nghĩa
   - `IOdometrySource` interface - Cần định nghĩa
   - `IMapProvider` interface - Cần định nghĩa
   - Integration với Pnkx Loc, odometry sources - Hiện tại dùng trực tiếp qua MoveBase methods

2. ⚠️ **Base Controller**:
   - `IBaseController` interface - Cần định nghĩa
   - Diff drive controller implementation - Cần implement
   - Steer drive controller implementation - Cần implement
   - Velocity command execution - Hiện tại chỉ có `setTwistLinear/Angular` trong local planner

3. ⚠️ **User Controller Plugin System**:
   - Factory để load user controller plugins - Cần implement
   - Interface cho User Controller - Cần định nghĩa
   - Integration với BaseNavigation - Cần implement

4. ⚠️ **Testing & Documentation**:
   - Unit tests cho các modules - Một số đã có (costmap, bounds, etc.)
   - Integration tests - Cần bổ sung
   - Simulation environment - TODO

**Lộ trình tiếp theo:**

**Phase 1: Data Sources & Base Controller** (Ưu tiên cao)
- Định nghĩa interfaces cho data sources
- Implement Base Controller interface và diff drive controller
- Integration với MoveBase

**Phase 2: Control Loop & State Management** (Ưu tiên cao)
- Hoàn thiện executeCycle trong MoveBase
- State machine management
- Threading strategy

**Phase 3: User Controller Plugin System** (Ưu tiên trung bình)
- Factory pattern cho user controllers
- Plugin loading mechanism
- Integration testing

**Phase 4: Testing & Optimization** (Ưu tiên trung bình)
- Unit tests cho các module
- Integration tests
- Performance optimization

**Testing strategy:**
- Unit tests cho các module độc lập (gtest?)
- Integration tests cho full navigation stack
- Simulation environment (2D simulator) - TODO
- Hardware-in-the-loop testing - TODO

## Cấu trúc thư mục

```
pnkx_nav_core/
├── src/
│   ├── Navigations/
│   │   ├── Cores/
│   │   │   ├── move_base_core/              # BaseNavigation interface ✅
│   │   │   │   ├── include/move_base_core/
│   │   │   │   │   ├── navigation.h         # BaseNavigation abstract class
│   │   │   │   │   └── common.h             # Common types (TFListenerPtr, etc.)
│   │   │   ├── robot_nav_core/              # Planner interfaces ✅
│   │   │   │   ├── include/robot_nav_core/
│   │   │   │   │   ├── base_global_planner.h
│   │   │   │   │   ├── base_local_planner.h
│   │   │   │   │   └── recovery_behavior.h
│   │   │   ├── robot_nav_core_adapter/      # Adapter utilities ✅
│   │   │   │   ├── include/robot_nav_core_adapter/
│   │   │   │   │   ├── global_planner_adapter.h
│   │   │   │   │   ├── local_planner_adapter.h
│   │   │   │   │   └── costmap_adapter.h
│   │   │   └── robot_nav_core2/             # Additional nav utilities ✅
│   │   │       ├── include/robot_nav_core2/
│   │   │       │   ├── global_planner.h
│   │   │       │   ├── local_planner.h
│   │   │       │   └── costmap.h
│   │   ├── Libraries/
│   │   │   ├── nav_grid/                    # Navigation grid utilities ✅
│   │   │   └── (costmap, tf3, robot_time ở Libraries/)
│   │   └── Packages/
│   │       └── move_base/                   # MoveBase implementation ✅
│   │           ├── include/move_base/
│   │           │   └── move_base.h
│   │           └── src/
│   │               ├── move_base.cpp
│   │               └── move_base_main.cpp
│   │
│   ├── Algorithms/
│   │   ├── Cores/
│   │   │   └── score_algorithm/             # Trajectory scoring ✅
│   │   │       ├── include/score_algorithm/
│   │   │       │   ├── score_algorithm.h
│   │   │       │   ├── goal_checker.h
│   │   │       │   └── trajectory_generator.h
│   │   ├── Libraries/
│   │   │   ├── mkt_algorithm/               # MKT kinematics algorithms ✅
│   │   │   │   ├── include/mkt_algorithm/
│   │   │   │   │   ├── diff/                # Differential drive
│   │   │   │   │   │   ├── diff_go_straight.h
│   │   │   │   │   │   ├── diff_predictive_trajectory.h
│   │   │   │   │   │   └── diff_rotate_to_goal.h
│   │   │   │   │   └── bicycle/             # Bicycle model
│   │   │   │   │       ├── bicycle.h
│   │   │   │   │       ├── go_straight.h
│   │   │   │   │       └── rotate_to_goal.h
│   │   │   ├── mkt_plugins/                 # MKT plugin components ✅
│   │   │   │   ├── include/mkt_plugins/
│   │   │   │   │   ├── goal_checker.h
│   │   │   │   │   ├── standard_traj_generator.h
│   │   │   │   │   ├── kinematic_parameters.h
│   │   │   │   │   └── xy_theta_iterator.h
│   │   │   ├── mkt_msgs/                    # MKT message types ✅
│   │   │   ├── kalman/                      # Kalman filtering ✅
│   │   │   └── angles/                      # Angle utilities ✅
│   │   └── Packages/
│   │       ├── global_planners/
│   │       │   ├── custom_planner/           # Custom global planner ✅
│   │       │   ├── dock_planner/            # Docking planner ✅
│   │       │   └── two_points_planner/      # Two points planner ✅
│   │       └── local_planners/
│   │           └── pnkx_local_planner/      # PNKX local planner ✅
│   │               ├── include/pnkx_local_planner/
│   │               │   ├── pnkx_local_planner.h
│   │               │   ├── pnkx_docking_local_planner.h
│   │               │   ├── pnkx_go_straight_local_planner.h
│   │               │   └── pnkx_rotate_local_planner.h
│   │
│   ├── Libraries/
│   │   ├── costmap_2d/                      # Costmap system ✅
│   │   │   ├── include/robot_costmap_2d/
│   │   │   │   ├── costmap_2d_robot.h
│   │   │   │   ├── layered_costmap.h
│   │   │   │   └── layer.h
│   │   │   ├── plugins/                     # Costmap layers ✅
│   │   │   │   ├── static_layer.cpp
│   │   │   │   ├── obstacle_layer.cpp
│   │   │   │   ├── inflation_layer.cpp
│   │   │   │   ├── voxel_layer.cpp
│   │   │   │   ├── critical_layer.cpp
│   │   │   │   ├── directional_layer.cpp
│   │   │   │   ├── preferred_layer.cpp
│   │   │   │   └── unpreferred_layer.cpp
│   │   │   └── config/                      # Costmap configs
│   │   ├── tf3/                             # Transform system ✅
│   │   ├── robot_time/                      # Time management ✅
│   │   ├── robot_cpp/                       # Core utilities ✅
│   │   │   ├── include/robot/
│   │   │   │   ├── init.h                   # Initialization
│   │   │   │   ├── node_handle.h            # NodeHandle (YAML config)
│   │   │   │   └── plugin_loader_helper.h   # Plugin loader
│   │   ├── robot_nav_2d_utils/              # 2D nav utilities ✅
│   │   ├── robot_nav_2d_msgs/               # 2D nav messages ✅
│   │   ├── geometry2/                       # Geometry utilities ✅
│   │   │   ├── robot_tf3_geometry_msgs/
│   │   │   └── robot_tf3_sensor_msgs/
│   │   ├── common_msgs/                     # Message types ✅
│   │   │   ├── robot_geometry_msgs/
│   │   │   ├── robot_nav_msgs/
│   │   │   ├── robot_sensor_msgs/
│   │   │   ├── robot_std_msgs/
│   │   │   ├── robot_protocol_msgs/
│   │   │   └── robot_visualization_msgs/
│   │   ├── laser_geometry/                  # Laser geometry ✅
│   │   ├── voxel_grid/                      # Voxel grid ✅
│   │   ├── data_convert/                    # Data conversion ✅
│   │   └── xmlrpcpp/                        # XML-RPC utilities ✅
│   │
│   └── APIs/
│       └── c_api/                           # C API wrapper ✅
│           ├── include/nav_c_api.h
│           └── src/nav_c_api.cpp
│
├── build/                                   # Build artifacts
├── config/                                  # Configuration files
├── examples/                                # Example code
└── doc/                                     # Documentation
    ├── architecture_discussion.md
    ├── implementation_plan.md
    └── folders.md
```

## Ghi chú

- ✅ Kiến trúc cốt lõi đã được triển khai với plugin system linh hoạt
- ✅ **3 Global Planners** đã triển khai: custom_planner, dock_planner, two_points_planner
- ✅ **1 Local Planner** đã triển khai: pnkx_local_planner với 4 behaviors (position, docking, go_straight, rotate)
- ✅ **8 Costmap Layers** đã triển khai: static, obstacle, inflation, voxel, critical, directional, preferred, unpreferred
- ✅ **MKT Algorithms** hỗ trợ cả differential drive và bicycle model
- ✅ **Plugin System** hoàn chỉnh với `boost::dll` và `robot::PluginLoaderHelper`
- ⚠️ Cần bổ sung data sources interfaces và base controller
- 🔄 Kiến trúc được thiết kế để dễ dàng thay đổi thuật toán và mô hình kinematics thông qua plugin system
- 📦 Tất cả components được build bằng CMake, hỗ trợ cả Catkin và Standalone CMake
- 🔌 Plugin system sử dụng `boost::dll` cho dynamic loading
- 🎯 MoveBase đã có control loop (`executeCycle`) và state machine hoàn chỉnh
- 📚 Có đầy đủ message types: geometry_msgs (29), nav_msgs (24), sensor_msgs (30), std_msgs (32)