# 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