# 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