# score_algorithm

## Giới thiệu
`score_algorithm` là lớp nền (base class) cho các thuật toán đánh giá và sinh lệnh điều khiển (local planning) trong `pnkx_nav_core`. Module này không tự sinh quỹ đạo cụ thể, mà cung cấp bộ khung và các hàm dùng chung để các thuật toán con triển khai.

## Vai trò chính
- Chuẩn hóa giao diện (interface) cho các thuật toán tính toán quỹ đạo/lệnh.
- Cắt đoạn `global_plan` thành đoạn con phù hợp với `local costmap`.
- Chọn `sub-goal` và `start` để theo đuổi theo từng bước đường đi.
- Cung cấp các hàm tiện ích: kiểm tra dừng/đi, tính quãng đường, tìm mục tiêu theo góc và khoảng cách.

## API chính
Được khai báo trong `include/score_algorithm/score_algorithm.h`:
- `initialize(...)`: khởi tạo tham số, TF, costmap, bộ sinh quỹ đạo.
- `prepare(...)`: chuẩn bị dữ liệu dùng chung trước khi đánh giá.
- `calculator(...)`: tính toán ra `mkt_msgs::Trajectory2D`.
- `computePlanCommand(...)`: cắt `global_plan` thành đoạn kết quả và chọn `start/goal`.
- `findSubGoalIndex(...)`, `getGoalIndex(...)`, `journey(...)`, `stopped(...)`: hàm phụ trợ.

## Nguyên lý `computePlanCommand(...)`
Hàm này nhận `robot_pose`, `velocity`, độ dài xem trước `S` và `global_plan`, sau đó:
1) Tìm các mốc `sub-goal` (đổi hướng / điểm gãy) bằng `findSubGoalIndex`.
2) Khôi phục `sub_goal_index` và `sub_start_index` đã lưu (nếu có), cập nhật nếu robot đang dừng và đã vượt mốc.
3) Tìm `start_index` gần nhất với robot trong đoạn con hợp lệ.
4) Tìm `goal_index` dựa trên quy tắc "đi đủ xa `S` hoặc đổi hướng vượt ngưỡng".
5) Xuất `result` là đoạn từ `closet_index` đến `goal_index`.

Kết quả được sử dụng bởi các local planner để sinh lệnh điều khiển tiếp theo.

## Sử dụng trong hệ thống
`score_algorithm` được nạp qua pluginlib trong các local planner, ví dụ:
- `pnkx_local_planner`
- `pnkx_go_straight_local_planner`
- `pnkx_rotate_local_planner`
- `pnkx_docking_local_planner`

Mỗi thuật toán cụ thể sẽ kế thừa `ScoreAlgorithm` và triển khai `initialize`, `prepare`, `calculator`.

## Thư mục liên quan
- `include/score_algorithm/score_algorithm.h`: giao diện chính.
- `src/score_algorithm.cpp`: các hàm dùng chung (getGoalIndex, computePlanCommand, ...).

## Ghi chú
Module này phụ thuộc vào `robot_nav_2d_msgs`, `robot_costmap_2d`, `mkt_msgs` và hệ thống TF/ROS.
