# move_base_core

## Tổng quan

`move_base_core` là một thư viện header-only C++ cung cấp các interface và cấu trúc dữ liệu cốt lõi cho hệ thống navigation của robot, tương tự như `move_base_msgs` trong ROS nhưng độc lập, không phụ thuộc vào ROS. Thư viện này định nghĩa:

- **Interface chính**: `BaseNavigation` - giao diện trừu tượng cho các module navigation
- **Trạng thái navigation**: Enum `State` mô tả các trạng thái của quá trình navigation
- **Feedback structure**: `NavFeedback` để theo dõi trạng thái navigation
- **Planner data**: `PlannerDataOutput` cho dữ liệu đầu ra từ planner
- **Utility functions**: Các hàm hỗ trợ chuyển đổi và tính toán

## Mục đích

Thư viện này được thiết kế để:

- Cung cấp API chuẩn hóa cho navigation system không sử dụng ROS
- Định nghĩa interface chung cho các implementation navigation khác nhau
- Hỗ trợ các chức năng navigation cơ bản: di chuyển đến điểm, docking, quay, tạm dừng
- Cung cấp cơ chế feedback và trạng thái để theo dõi quá trình navigation
- Đảm bảo tính nhất quán và khả năng mở rộng cho các module navigation

## Các thành phần chính

### 1. `BaseNavigation` - Interface chính cho Navigation

`BaseNavigation` là abstract class định nghĩa tất cả các phương thức cần thiết cho một hệ thống navigation hoàn chỉnh.

**Chức năng chính:**

- **Khởi tạo**: `initialize()` - khởi tạo hệ thống navigation với TF listener
- **Quản lý footprint**: `setRobotFootprint()`, `getRobotFootprint()` - thiết lập và lấy hình dạng robot
- **Quản lý dữ liệu cảm biến**: 
  - Thêm/xóa static maps, laser scans, point clouds
  - Quản lý nhiều nguồn dữ liệu với tên riêng biệt
- **Quản lý odometry**: `addOdometry()` - cập nhật thông tin vị trí robot
- **Navigation commands**:
  - `moveTo()` - di chuyển đến một điểm đích
  - `dockTo()` - docking đến marker (ví dụ: trạm sạc)
  - `moveStraightTo()` - di chuyển thẳng đến đích
  - `rotateTo()` - quay tại chỗ đến hướng mong muốn
- **Điều khiển motion**:
  - `pause()` - tạm dừng di chuyển
  - `resume()` - tiếp tục di chuyển
  - `cancel()` - hủy mục tiêu hiện tại
  - `setTwistLinear()`, `setTwistAngular()` - điều khiển vận tốc
- **Lấy thông tin**:
  - `getRobotPose()` - lấy vị trí hiện tại của robot
  - `getTwist()` - lấy vận tốc hiện tại
  - `getFeedback()` - lấy feedback về trạng thái navigation
  - `getGlobalData()`, `getLocalData()` - lấy dữ liệu từ planner

**Đặc điểm:**

- Tất cả các phương thức đều là pure virtual, đảm bảo implementation đầy đủ
- Hỗ trợ nhiều nguồn dữ liệu cảm biến với tên riêng biệt
- Cung cấp cả PoseStamped và Pose2D cho linh hoạt
- Thread-safe design với shared pointers

### 2. `State` - Trạng thái Navigation

Enum `State` mô tả các trạng thái khác nhau trong quá trình navigation.

**Các trạng thái chính:**

- **PENDING**: Chờ xử lý, mục tiêu đã được gửi nhưng chưa bắt đầu
- **ACTIVE**: Đang hoạt động, navigation đang được thực thi
- **PLANNING**: Đang lập kế hoạch đường đi
- **CONTROLLING**: Đang điều khiển robot di chuyển theo plan
- **CLEARING**: Đang dọn dẹp costmap, thực hiện recovery behaviors
- **PAUSED**: Tạm dừng navigation
- **SUCCEEDED**: Hoàn thành thành công
- **ABORTED**: Bị lỗi, không thể hoàn thành
- **PREEMPTED**: Bị hủy bởi mục tiêu mới
- **REJECTED**: Bị từ chối bởi planner hoặc controller
- **LOST**: Mất trạng thái, không xác định được vị trí

**Ứng dụng:**

- Theo dõi tiến trình navigation
- Debugging và logging
- Hiển thị trạng thái trong UI
- Điều khiển state machine của navigation system

### 3. `NavFeedback` - Feedback Navigation

Cấu trúc `NavFeedback` cung cấp thông tin chi tiết về trạng thái navigation hiện tại.

**Các thành phần:**

- **navigation_state**: Trạng thái hiện tại của navigation (State enum)
- **feed_back_str**: Chuỗi mô tả hoặc thông điệp debug
- **current_pose**: Vị trí hiện tại của robot (Pose2D)
- **goal_checked**: Cờ xác nhận mục tiêu đã được kiểm tra
- **is_ready**: Cờ cho biết robot đã sẵn sàng nhận lệnh

**Ứng dụng:**

- Cập nhật trạng thái real-time cho người dùng
- Debugging và monitoring
- Ghi log quá trình navigation
- Tích hợp với hệ thống giám sát

### 4. `PlannerDataOutput` - Dữ liệu Planner

Cấu trúc `PlannerDataOutput` chứa dữ liệu đầu ra từ planner.

**Các thành phần:**

- **plan**: Đường đi đã được lập kế hoạch (Path2D)
- **costmap**: Bản đồ chi phí (OccupancyGrid)
- **costmap_update**: Cập nhật bản đồ chi phí (OccupancyGridUpdate)

**Ứng dụng:**

- Lấy kế hoạch đường đi từ global/local planner
- Xem costmap để debugging
- Cập nhật costmap động
- Visualization và monitoring

### 5. Utility Functions - Hàm hỗ trợ

Các hàm tiện ích hỗ trợ các thao tác phổ biến.

**`toString(State state)`**:

- Chuyển đổi enum State sang chuỗi mô tả
- Hữu ích cho logging, debugging, hiển thị UI
- Trả về tên trạng thái dễ đọc (ví dụ: "PLANNING", "CONTROLLING")

**`offset_goal()`**:

- Tạo mục tiêu mới bằng cách offset từ vị trí hiện tại
- Hỗ trợ offset dọc theo hướng heading của robot
- Có hai overload: từ Pose2D và từ PoseStamped
- Hữu ích cho các tác vụ như di chuyển tiến/lùi một khoảng cách cố định

## Ứng dụng

### Trong Navigation System

`move_base_core` là nền tảng cho các implementation navigation như `move_base`:

- Định nghĩa contract mà tất cả navigation modules phải tuân theo
- Cho phép thay thế implementation mà không thay đổi code sử dụng
- Hỗ trợ plugin system cho các navigation strategies khác nhau

### Trong User Applications

Các ứng dụng sử dụng navigation system:

- Gọi các phương thức `moveTo()`, `dockTo()`, `rotateTo()` để điều khiển robot
- Theo dõi `NavFeedback` để biết trạng thái navigation
- Sử dụng `getRobotPose()` để lấy vị trí hiện tại
- Quản lý dữ liệu cảm biến thông qua các phương thức add/remove

### Trong Monitoring Systems

Hệ thống giám sát và logging:

- Sử dụng `State` để hiển thị trạng thái navigation
- Đọc `NavFeedback` để cập nhật UI real-time
- Sử dụng `toString()` để format log messages
- Lấy `PlannerDataOutput` để visualize costmap và plan

## Best Practices

### Implementation BaseNavigation

- Implement đầy đủ tất cả pure virtual methods
- Đảm bảo thread-safety cho các phương thức public
- Cập nhật `nav_feedback_` một cách nhất quán
- Xử lý lỗi một cách graceful và cập nhật state phù hợp

### Sử dụng State

- Luôn kiểm tra state trước khi thực hiện các thao tác
- Sử dụng `toString()` cho logging thay vì hardcode strings
- Xử lý các trạng thái lỗi (ABORTED, REJECTED) một cách phù hợp

### Quản lý Feedback

- Cập nhật `navigation_state` ngay khi state thay đổi
- Cung cấp `feed_back_str` mô tả rõ ràng cho debugging
- Đảm bảo `current_pose` được cập nhật thường xuyên
- Sử dụng `is_ready` để kiểm tra robot sẵn sàng nhận lệnh

### Quản lý Dữ liệu Cảm biến

- Sử dụng tên rõ ràng, duy nhất cho mỗi nguồn dữ liệu
- Xóa dữ liệu không còn sử dụng để tiết kiệm bộ nhớ
- Kiểm tra dữ liệu tồn tại trước khi sử dụng
- Cập nhật dữ liệu thường xuyên để đảm bảo tính chính xác

## Tính năng nổi bật

### Header-Only Library

- Không cần compile, chỉ cần include header files
- Dễ dàng tích hợp vào các project khác
- Không có runtime dependencies

### ROS-Independent

- Hoàn toàn độc lập với ROS
- Có thể sử dụng trong các ứng dụng C++ thuần
- Tương thích với các framework khác

### Extensible Design

- Dễ dàng mở rộng với các phương thức mới
- Hỗ trợ nhiều implementation khác nhau
- Plugin-friendly architecture

### Type-Safe

- Sử dụng strong typing với enums và structures
- Compile-time type checking
- Giảm thiểu lỗi runtime

## Dependencies

`move_base_core` phụ thuộc vào:

- **tf3**: Transform library cho coordinate frames
- **robot_time**: Time utilities
- **robot_geometry_msgs**: Geometry message types
- **robot_nav_2d_msgs**: 2D navigation message types
- **robot_nav_msgs**: Navigation message types
- **robot_sensor_msgs**: Sensor message types
- **robot_map_msgs**: Map message types
- **robot_protocol_msgs**: Protocol message types

## Tóm tắt

`move_base_core` là nền tảng quan trọng cho navigation system, cung cấp:

- Interface chuẩn hóa cho navigation modules
- Cấu trúc dữ liệu cho state và feedback
- Utility functions hỗ trợ các thao tác phổ biến
- Design linh hoạt, dễ mở rộng và không phụ thuộc ROS

Thư viện này đảm bảo tính nhất quán và khả năng tương thích giữa các component navigation khác nhau, tạo nền tảng vững chắc cho việc phát triển các ứng dụng navigation phức tạp.
