created

2025-12-05 11:12:17 +07:00
parent f60cbc2ed3
commit 45d965671e
196 changed files with 41817 additions and 0 deletions
--- a/doc/architecture_discussion.md
+++ b/doc/architecture_discussion.md
@@ -0,0 +1,446 @@
+# 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/>📦 costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
+    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 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/>📍 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/>📍 geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
+    Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
+    Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 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/>📤 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** ✅
+   - `nav_core::BaseGlobalPlanner`: Interface cho global planners (A*, D*, etc.)
+   - `nav_core::BaseLocalPlanner`: Interface cho local planners (DWA, TEB, MKT, etc.)
+   - `nav_core::RecoveryBehavior`: Interface cho recovery behaviors
+   - Plugin system sử dụng `boost::dll` để dynamic loading
+
+5. **Costmap Layer** ✅
+   - `costmap_2d::Costmap2DROBOT`: Global và local costmap
+   - Costmap layers: static map, obstacles, inflation
+   - Frame management: map (global), odom (local)
+
+6. **Algorithms Layer** ✅
+   - `mkt_algorithm`: Diff drive và bicycle kinematics algorithms
+   - `score_algorithm`: Trajectory scoring và goal checking
+   - `kalman`: Filtering algorithms
+
+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:**
+- `geometry_msgs::Pose2D` / `geometry_msgs::PoseStamped` (vị trí + hướng)
+- `geometry_msgs::Twist` (vận tốc linear/angular)
+- `std::vector<geometry_msgs::PoseStamped>` (đường đi)
+- `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
+
+- `nav_core::BaseGlobalPlanner` ✅
+  - `makePlan(start, goal, plan)` - Tạo global path
+  - `initialize(name, costmap_robot)` - Khởi tạo với costmap
+
+- `nav_core::BaseLocalPlanner` ✅
+  - `computeVelocityCommands(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
+  - `setTwistLinear/Angular(...)` - Set velocity limits
+
+- `nav_core::RecoveryBehavior` ✅
+  - `runBehavior()` - Thực thi recovery behavior
+
+- `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`
+- Config file YAML để specify plugin names
+- Plugin interfaces: `BaseGlobalPlanner`, `BaseLocalPlanner`, `RecoveryBehavior`
+
+### 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**: `BaseNavigation` interface
+2. ✅ **Implementation Layer**: `MoveBase` core logic
+3. ✅ **Planning Layer**: Plugin system cho global/local planners và recovery
+4. ✅ **Costmap Layer**: Global và local costmap với layers
+5. ✅ **Algorithms Layer**: MKT algorithms, score algorithm, kalman
+6. ✅ **API Layer**: C API wrapper cho .NET integration
+7. ✅ **Supporting Libraries**: tf3, robot_time, geometry_msgs, nav_2d_utils
+
+**Đang triển khai / Cần bổ sung:** ⚠️
+
+1. ⚠️ **Data Sources Interfaces**: 
+   - `ILocalizationSource` interface
+   - `IOdometrySource` interface
+   - `IMapProvider` interface
+   - Integration với Pnkx Loc, odometry sources
+
+2. ⚠️ **Base Controller**:
+   - `IBaseController` interface
+   - Diff drive controller implementation
+   - Steer drive controller implementation
+   - Velocity command execution
+
+3. ⚠️ **Control Loop**:
+   - Control loop trong MoveBase (executeCycle)
+   - State machine management hoàn chỉnh
+   - Threading và synchronization
+
+4. ⚠️ **User Controller Plugin System**:
+   - Factory để load user controller plugins
+   - Integration với BaseNavigation
+
+**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
+│   │   │   ├── nav_core/            # Planner interfaces
+│   │   │   ├── nav_core_adapter/    # Adapter utilities
+│   │   │   └── nav_core2/           # Additional nav utilities
+│   │   ├── Libraries/
+│   │   │   ├── costmap_2d/          # Costmap system
+│   │   │   ├── tf3/                 # Transform system
+│   │   │   ├── robot_time/          # Time management
+│   │   │   ├── geometry2/           # Geometry utilities
+│   │   │   └── ...                  # Other supporting libraries
+│   │   └── Packages/
+│   │       └── move_base/           # MoveBase implementation
+│   ├── Algorithms/
+│   │   ├── Cores/
+│   │   │   └── score_algorithm/     # Trajectory scoring
+│   │   └── Libraries/
+│   │       ├── mkt_algorithm/      # MKT kinematics algorithms
+│   │       ├── kalman/              # Kalman filtering
+│   │       └── angles/               # Angle utilities
+│   └── APIs/
+│       └── c_api/                   # C API wrapper
+├── build/                            # Build artifacts
+└── doc/                              # Documentation
+```
+
+## Ghi chú
+
+- ✅ Kiến trúc cốt lõi đã được triển khai với plugin system linh hoạt
+- ⚠️ 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, không phụ thuộc ROS
+- 🔌 Plugin system sử dụng `boost::dll` cho dynamic loading
+
+
+
+
+
+
--- a/doc/diff_drive_c.drawio
+++ b/doc/diff_drive_c.drawio
--- a/doc/folders.md
+++ b/doc/folders.md
@@ -0,0 +1,53 @@
+Mô tả cấu trúc:
+
+├── common_msgs/
+│   ├── build/
+│   ├── geometry_msgs/
+│   │   ├── include/
+│   │   └── test/
+│   ├── CMakeLists.txt
+│   ├── nav_msgs/
+│   │   ├── include/
+│   │   └── test/
+│   ├── CMakeLists.txt
+│   ├── sensor_msgs/
+│   │   ├── cfg/
+│   │   ├── include/
+│   │   └── test/
+│   ├── CMakeLists.txt
+│   ├── std_msgs/
+│   │   ├── include/
+│   │   └── CMakeLists.txt
+│   └── CMakeLists.txt (root)
+|
+├── Navigations/
+│   ├── Cores/
+│   │   └── move_base_core/
+│   │       ├── build/
+│   │       ├── example/
+│   │       ├── include/
+│   │       ├── .gitignore
+│   │       ├── CMakeLists.txt
+│   │       └── README.md
+│   ├── Libraries/
+│   │   ├── geometry_msgs/
+│   │   │   ├── build/
+│   │   │   ├── include/
+│   │   │   └── CMakeLists.txt
+│   │   └── libtf2/
+│   │       ├── .vscode/
+│   │       ├── include/
+│   │       ├── samples/
+│   │       ├── src/
+│   │       ├── .gitignore
+│   │       ├── DEBIAN_PACKAGING.md
+│   │       ├── libtf2_2.0.0_amd64.deb
+│   │       ├── Makefile
+│   │       └── README.md
+│   ├── Packages/
+│   │   └── move_base/
+│   │       ├── build/
+│   │       ├── include/
+│   │       ├── src/
+│   │       └── CMakeLists.txt
+│   └── CMakeLists.txt (root)
--- a/doc/implementation_plan.md
+++ b/doc/implementation_plan.md
@@ -0,0 +1,230 @@
+# Kế hoạch Implementation Navigation System
+
+## Phân tích Kiến trúc Hiện tại
+
+### ✅ Đã có sẵn:
+
+1. **Interface Layer:**
+   - `move_base_core::BaseNavigation` - Interface chính
+   - `move_base::MoveBase` - Implementation
+
+2. **Core Components:**
+   - `nav_core::BaseGlobalPlanner` - Interface cho global planner
+   - `nav_core::BaseLocalPlanner` - Interface cho local planner
+   - `nav_core::RecoveryBehavior` - Interface cho recovery behaviors
+   - Plugin system với `boost::dll` để load dynamic plugins
+
+3. **Costmap System:**
+   - `costmap_2d::Costmap2DROBOT` - Global và local costmap
+   - Costmap layers (static, obstacle, inflation, etc.)
+
+4. **Supporting Libraries:**
+   - `tf3` - Transform system
+   - `robot_time` - Time management
+   - `geometry_msgs` - Message types
+   - `nav_2d_msgs`, `nav_2d_utils` - 2D navigation utilities
+
+### ❌ Cần bổ sung/hoàn thiện:
+
+1. **User Controller Plugin System:**
+   - Factory để tạo User Controller từ plugin
+   - Interface cho User Controller
+   - Integration với BaseNavigation
+
+2. **Data Sources Integration:**
+   - Pnkx Loc interface
+   - Odometry Source interface
+   - Sensor Transform management
+   - Map Server interface
+   - Laser Sensor Sources interface
+
+3. **Base Controller:**
+   - Diff drive controller
+   - Steer drive controller
+   - Velocity command execution
+
+4. **Hoàn thiện MoveBase:**
+   - Control loop implementation
+   - State machine management
+   - Recovery behavior execution
+
+## Kế hoạch Implementation
+
+### Phase 1: Data Sources & Interfaces (Ưu tiên cao)
+
+#### 1.1 Odometry Source Interface
+**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/odometry_source.h`
+```cpp
+class IOdometrySource {
+  virtual geometry_msgs::PoseStamped getCurrentPose() = 0;
+  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
+  virtual bool isAvailable() = 0;
+};
+```
+
+#### 1.2 Localization Source Interface (Pnkx Loc)
+**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/localization_source.h`
+```cpp
+class ILocalizationSource {
+  virtual geometry_msgs::PoseStamped getRobotPose() = 0;
+  virtual std::string getGlobalFrame() = 0;
+  virtual bool isLocalized() = 0;
+};
+```
+
+#### 1.3 Map Provider Interface
+**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/map_provider.h`
+```cpp
+class IMapProvider {
+  virtual nav_msgs::OccupancyGrid getMap() = 0;
+  virtual bool isMapAvailable() = 0;
+};
+```
+
+#### 1.4 Sensor Data Interface
+**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/sensor_source.h`
+```cpp
+class ISensorSource {
+  virtual sensor_msgs::LaserScan getLatestScan() = 0;
+  virtual bool hasNewData() = 0;
+};
+```
+
+### Phase 2: Base Controller (Ưu tiên cao)
+
+#### 2.1 Base Controller Interface
+**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/base_controller.h`
+```cpp
+class IBaseController {
+  virtual bool setVelocity(const geometry_msgs::Twist& cmd_vel) = 0;
+  virtual bool stop() = 0;
+  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
+};
+```
+
+#### 2.2 Diff Drive Controller
+**File:** `src/Navigations/Packages/base_controllers/include/base_controllers/diff_drive_controller.h`
+- Implement IBaseController
+- Convert Twist to wheel velocities
+- Handle kinematics cho differential drive
+
+#### 2.3 Steer Drive Controller
+**File:** `src/Navigations/Packages/base_controllers/include/base_controllers/steer_drive_controller.h`
+- Implement IBaseController
+- Convert Twist to steering angles và wheel velocities
+- Handle kinematics cho bicycle/steering model
+
+### Phase 3: User Controller Plugin System (Ưu tiên trung bình)
+
+#### 3.1 User Controller Interface
+**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/user_controller.h`
+```cpp
+class IUserController {
+  virtual void onGoalReceived(const geometry_msgs::PoseStamped& goal) = 0;
+  virtual void onNavigationStateChanged(move_base_core::State state) = 0;
+  virtual void onFeedback(const NavFeedback& feedback) = 0;
+};
+```
+
+#### 3.2 User Controller Factory
+**File:** `src/Navigations/Cores/move_base_core/src/user_controller_factory.cpp`
+- Load plugin từ config
+- Create instance với boost::dll
+- Manage lifecycle
+
+### Phase 4: Control Loop & State Machine (Ưu tiên cao)
+
+#### 4.1 Control Loop Implementation
+**File:** `src/Navigations/Packages/move_base/src/move_base_control_loop.cpp`
+- Main control loop thread
+- Planner thread management
+- Controller execution
+- Frequency control
+
+#### 4.2 State Machine
+**File:** `src/Navigations/Packages/move_base/src/move_base_state_machine.cpp`
+- State transitions
+- Recovery behavior triggers
+- Goal management
+
+### Phase 5: Integration & Testing (Ưu tiên trung bình)
+
+#### 5.1 Integration Layer
+- Connect tất cả components
+- Data flow management
+- Error handling
+
+#### 5.2 Testing
+- Unit tests
+- Integration tests
+- Simulation tests
+
+## Cấu trúc Thư mục Đề xuất
+
+```
+src/Navigations/
+├── Cores/
+│   ├── move_base_core/          ✅ Đã có
+│   │   ├── interfaces/
+│   │   │   ├── odometry_source.h      ⚠️ Cần tạo
+│   │   │   ├── localization_source.h ⚠️ Cần tạo
+│   │   │   ├── map_provider.h         ⚠️ Cần tạo
+│   │   │   ├── sensor_source.h        ⚠️ Cần tạo
+│   │   │   ├── base_controller.h      ⚠️ Cần tạo
+│   │   │   └── user_controller.h      ⚠️ Cần tạo
+│   │   └── src/
+│   │       └── user_controller_factory.cpp ⚠️ Cần tạo
+│   ├── nav_core/                ✅ Đã có
+│   └── nav_core2/               ✅ Đã có
+│
+├── Packages/
+│   ├── move_base/               ✅ Đã có (cần hoàn thiện)
+│   │   └── src/
+│   │       ├── move_base_control_loop.cpp  ⚠️ Cần tạo
+│   │       └── move_base_state_machine.cpp ⚠️ Cần tạo
+│   │
+│   └── base_controllers/        ⚠️ Cần tạo
+│       ├── include/
+│       │   └── base_controllers/
+│       │       ├── diff_drive_controller.h
+│       │       └── steer_drive_controller.h
+│       └── src/
+│           ├── diff_drive_controller.cpp
+│           └── steer_drive_controller.cpp
+│
+└── Libraries/
+    └── (các libraries hiện có) ✅
+```
+
+## Thứ tự Implementation
+
+### Sprint 1: Foundation (1-2 tuần)
+1. ✅ Tạo interfaces cho data sources
+2. ✅ Tạo base controller interface
+3. ✅ Implement diff drive controller cơ bản
+
+### Sprint 2: Integration (1-2 tuần)
+1. ✅ Integrate data sources vào MoveBase
+2. ✅ Implement control loop
+3. ✅ Connect base controller
+
+### Sprint 3: Plugin System (1 tuần)
+1. ✅ User controller plugin system
+2. ✅ Factory pattern implementation
+
+### Sprint 4: State Machine & Recovery (1 tuần)
+1. ✅ State machine implementation
+2. ✅ Recovery behavior integration
+
+### Sprint 5: Testing & Polish (1 tuần)
+1. ✅ Unit tests
+2. ✅ Integration tests
+3. ✅ Documentation
+
+## Notes
+
+- Tất cả interfaces nên follow pattern của `BaseNavigation`
+- Sử dụng `boost::dll` cho plugin system (đã có sẵn)
+- Thread safety: sử dụng `boost::mutex` và `boost::recursive_mutex`
+- Error handling: throw exceptions, return bool cho simple operations
+
--- a/doc/readme.md
+++ b/doc/readme.md
@@ -0,0 +1,15 @@
+Mô tả dựng án :
+ Tôi đang nghiên cứu viết chương di chuyển bám quỹ đạo (navigation) cho robot amr dạng hai bánh vi sai. Tôi muốn bỏ ROS chuyển sang viết 
+ thuần trên Cmake.
+
+Các chức năng cần có:
+- Có khả năng thay đổi mô hình kinematics như dạng hai bánh vi sai, dạng steering bicycle, ...
+- Có chức năng di chuyển từ điểủ 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 
+
+
+
+
+