BQP/srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/README.md

# RobotNet10.GlobalPathPlanner

Thư viện path planning cho hệ thống robot, cung cấp các thuật toán tìm đường tối ưu trên graph với hỗ trợ nhiều loại robot khác nhau.

## 📋 Mục lục

- [Tổng quan](#tổng-quan)
- [Cài đặt](#cài-đặt)
- [Bắt đầu nhanh](#bắt-đầu-nhanh)
- [Hướng dẫn sử dụng](#hướng-dẫn-sử-dụng)
- [Các loại Path Planner](#các-loại-path-planner)
- [Configuration Options](#configuration-options)
- [API Reference](#api-reference)
- [Ví dụ nâng cao](#ví-dụ-nâng-cao)
- [Best Practices](#best-practices)
- [Troubleshooting](#troubleshooting)

## 🎯 Tổng quan

`RobotNet10.GlobalPathPlanner` là một thư viện .NET cung cấp các thuật toán path planning cho robot, bao gồm:

- **A* Algorithm**: Thuật toán tìm đường tối ưu trên graph
- **SSE A* Algorithm**: State-Space Enhanced A* cho forklift robots
- **Hỗ trợ nhiều loại robot**: Differential drive, Forklift, Omni-directional drive
- **Bezier Curve Support**: Hỗ trợ edges với curves bậc 1, 2, hoặc 3
- **KD-Tree Optimization**: Tối ưu hóa tìm kiếm nearest neighbor
- **Cancellation & Timeout**: Hỗ trợ hủy bỏ và timeout cho operations

## 📦 Cài đặt

### Thêm Project Reference

Thêm reference đến project trong file `.csproj`:

```xml
<ItemGroup>
  <ProjectReference Include="path/to/RobotNet10.GlobalPathPlanner.csproj" />
</ItemGroup>
```

### Using Statements

```csharp
using RobotNet10.GlobalPathPlanner;
using RobotNet10.GlobalPathPlanner.Model;
```

## 🚀 Bắt đầu nhanh

### Ví dụ cơ bản

```csharp
using RobotNet10.GlobalPathPlanner;
using RobotNet10.GlobalPathPlanner.Model;

// 1. Tạo factory và planner
var factory = new PathPlannerFactory();
var planner = factory.CreateDifferentialPlanner();

// 2. Chuẩn bị dữ liệu map (nodes và edges)
var nodes = new GlobalNode[]
{
    new GlobalNode { Id = Guid.NewGuid(), X = 0, Y = 0, Name = "Start" },
    new GlobalNode { Id = Guid.NewGuid(), X = 10, Y = 10, Name = "Goal" }
};

var edges = new GlobalEdge[]
{
    new GlobalEdge 
    { 
        Id = Guid.NewGuid(), 
        StartNodeId = nodes[0].Id, 
        EndNodeId = nodes[1].Id,
        Degree = 1 // Linear edge
    }
};

// 3. Set data cho planner
planner.SetData(nodes, edges);

// 4. Tính toán đường đi
var (pathNodes, pathEdges) = planner.PathPlanning(
    x: 0.0, 
    y: 0.0, 
    theta: 0.0, 
    goalId: nodes[1].Id
);

// 5. Sử dụng kết quả
foreach (var node in pathNodes)
{
    Console.WriteLine($"Node: {node.Name} at ({node.X}, {node.Y})");
}
```

## 📖 Hướng dẫn sử dụng

### Workflow cơ bản

1. **Tạo Planner**: Sử dụng `PathPlannerFactory` để tạo planner phù hợp với loại robot
2. **Set Data**: Gọi `SetData()` để load map data (nodes và edges)
3. **Configure Options** (Optional): Gọi `SetOptions()` để cấu hình
4. **Path Planning**: Gọi các method path planning để tính toán đường đi
5. **Xử lý kết quả**: Sử dụng mảng nodes và edges trả về

### Tạo Path Planner

```csharp
var factory = new PathPlannerFactory();

// Cho differential drive robot
var differentialPlanner = factory.CreateDifferentialPlanner();

// Cho forklift robot (version 1)
var forkliftPlanner = factory.CreateForkliftPlanner();

// Cho forklift robot (version 2 - enhanced)
var forkliftPlannerV2 = factory.CreateForkliftPlannerV2();

// Cho omni-directional drive robot
var omniPlanner = factory.CreateOmniDrivePlanner();
```

### Load Map Data

```csharp
// Chuẩn bị nodes (waypoints)
var nodes = new GlobalNode[]
{
    new GlobalNode 
    { 
        Id = Guid.Parse("..."), 
        MapId = Guid.Parse("..."),
        X = 0.0, 
        Y = 0.0, 
        Name = "Node1",
        Orientation = Orientation.FORWARD
    },
    // ... thêm các nodes khác
};

// Chuẩn bị edges (connections)
var edges = new GlobalEdge[]
{
    new GlobalEdge 
    { 
        Id = Guid.Parse("..."),
        MapId = Guid.Parse("..."),
        StartNodeId = nodes[0].Id,
        EndNodeId = nodes[1].Id,
        Degree = 2, // Bezier curve bậc 2
        ControlPoint1X = 5.0,
        ControlPoint1Y = 5.0,
        ControlPoint2X = 0.0,
        ControlPoint2Y = 0.0
    },
    // ... thêm các edges khác
};

// Set data cho planner
planner.SetData(nodes, edges);
```

### Path Planning Methods

#### 1. Path Planning cơ bản (từ tọa độ)

```csharp
var (nodes, edges) = planner.PathPlanning(
    x: 5.0,           // Starting X coordinate
    y: 5.0,           // Starting Y coordinate
    theta: 45.0,       // Starting orientation (degrees)
    goalId: goalNodeId // Goal node ID
);
```

#### 2. Path Planning từ Node ID

```csharp
var (nodes, edges) = planner.PathPlanning(
    startNodeId: startNodeId, // Starting node ID
    theta: 45.0,              // Current orientation
    goalId: goalNodeId        // Goal node ID
);
```

#### 3. Path Planning với Starting Direction

```csharp
var (nodes, edges) = planner.PathPlanningWithStartDirection(
    x: 5.0,
    y: 5.0,
    theta: 45.0,
    goalId: goalNodeId,
    startDiretion: Orientation.FORWARD // FORWARD, BACKWARD, or NONE
);
```

#### 4. Path Planning với Final Direction

```csharp
var (nodes, edges) = planner.PathPlanningWithFinalDirection(
    x: 5.0,
    y: 5.0,
    theta: 45.0,
    goalId: goalNodeId,
    goalDirection: Orientation.BACKWARD // FORWARD, BACKWARD, or NONE
);
```

#### 5. Path Planning với Final Angle

```csharp
var (nodes, edges) = planner.PathPlanningWithAngle(
    x: 5.0,
    y: 5.0,
    theta: 45.0,
    goalId: goalNodeId,
    goalAngle: 90.0 // Desired final angle in degrees
);
```

### Sử dụng Cancellation Token

```csharp
using var cts = new CancellationTokenSource();

// Set timeout
cts.CancelAfter(TimeSpan.FromSeconds(5));

try
{
    var (nodes, edges) = planner.PathPlanning(
        x: 5.0,
        y: 5.0,
        theta: 45.0,
        goalId: goalNodeId,
        cancellationToken: cts.Token
    );
}
catch (OperationCanceledException)
{
    Console.WriteLine("Path planning was cancelled");
}
catch (TimeoutException)
{
    Console.WriteLine("Path planning timed out");
}
```

## 🤖 Các loại Path Planner

### 1. DifferentialPlanner

**Sử dụng cho:**
- Differential drive robots (2 bánh độc lập)
- Omni-directional drive robots

**Đặc điểm:**
- Sử dụng A* algorithm cơ bản
- Tính toán orientation (FORWARD/BACKWARD) tự động
- Default options phù hợp cho hầu hết các trường hợp

**Ví dụ:**
```csharp
var planner = factory.CreateDifferentialPlanner();
```

### 2. ForkliftPathPlanner

**Sử dụng cho:**
- Forklift robots (phiên bản 1)

**Đặc điểm:**
- Xử lý các ràng buộc đặc thù của forklift
- Tính toán turning radius phù hợp

**Ví dụ:**
```csharp
var planner = factory.CreateForkliftPlanner();
```

### 3. ForkLiftPathPlannerV2

**Sử dụng cho:**
- Forklift robots (phiên bản 2 - enhanced)

**Đặc điểm:**
- Sử dụng SSE A* algorithm (State-Space Enhanced A*)
- Hiệu năng và chất lượng đường đi tốt hơn
- Khuyến nghị sử dụng cho forklift

**Ví dụ:**
```csharp
var planner = factory.CreateForkliftPlannerV2();
```

### 4. OmniDrivePlanner

**Sử dụng cho:**
- Omni-directional drive robots

**Lưu ý:** Hiện tại sử dụng cùng planner với DifferentialPlanner

**Ví dụ:**
```csharp
var planner = factory.CreateOmniDrivePlanner();
```

## ⚙️ Configuration Options

### PathPlannerOptions

```csharp
var options = new PathPlannerOptions
{
    // Khoảng cách tối đa đến edge để được coi là "gần" edge
    LimitDistanceToEdge = 1.0,
    
    // Khoảng cách tối đa đến node để được coi là "tại" node
    LimitDistanceToNode = 0.3,
    
    // Độ phân giải khi split path (khoảng cách giữa các điểm)
    ResolutionSplit = 0.1,
    
    // Timeout cho path planning operation
    TimeOut = TimeSpan.FromSeconds(10),
    
    // Góc (degrees) để quyết định đổi hướng (FORWARD/BACKWARD)
    ChangeOrientationAngle = 89.0
};

planner.SetOptions(options);
```

### Giải thích các tham số

| Tham số | Mô tả | Giá trị mặc định | Đơn vị |
|---------|-------|------------------|--------|
| `LimitDistanceToEdge` | Khoảng cách tối đa để được coi là gần edge | 1.0 | meters |
| `LimitDistanceToNode` | Khoảng cách tối đa để được coi là tại node | 0.3 | meters |
| `ResolutionSplit` | Độ phân giải khi chia nhỏ path | 0.1 | meters |
| `TimeOut` | Timeout cho operation | null (no timeout) | TimeSpan |
| `ChangeOrientationAngle` | Góc để quyết định đổi hướng | 89.0 | degrees |

## 📚 API Reference

### IPathPlanner Interface

#### SetData

```csharp
void SetData(GlobalNode[] nodes, GlobalEdge[] edges)
```

Thiết lập dữ liệu graph (nodes và edges) cho planner. **Phải gọi trước khi thực hiện path planning.**

#### SetOptions

```csharp
void SetOptions(PathPlannerOptions options)
```

Cấu hình options cho planner. **Optional**, nếu không gọi sẽ dùng default options.

#### PathPlanning (từ tọa độ)

```csharp
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(
    double x, 
    double y, 
    double theta, 
    Guid goalId, 
    CancellationToken? cancellationToken = null
)
```

Tính toán đường đi từ tọa độ (x, y) đến goal node.

#### PathPlanning (từ Node ID)

```csharp
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(
    Guid startNodeId, 
    double theta, 
    Guid goalId, 
    CancellationToken? cancellationToken = null
)
```

Tính toán đường đi từ start node đến goal node. **Hiệu quả hơn** khi robot đã ở tại một node đã biết.

#### PathPlanningWithStartDirection

```csharp
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(
    double x, 
    double y, 
    double theta, 
    Guid goalId, 
    Orientation startDiretion = Orientation.NONE, 
    CancellationToken? cancellationToken = null
)
```

Tính toán đường đi với ràng buộc hướng bắt đầu.

#### PathPlanningWithFinalDirection

```csharp
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(
    double x, 
    double y, 
    double theta, 
    Guid goalId, 
    Orientation goalDirection = Orientation.NONE, 
    CancellationToken? cancellationToken = null
)
```

Tính toán đường đi với ràng buộc hướng kết thúc.

#### PathPlanningWithAngle

```csharp
(GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(
    double x, 
    double y, 
    double theta, 
    Guid goalId, 
    double goalAngle, 
    CancellationToken? cancellationToken = null
)
```

Tính toán đường đi với ràng buộc góc kết thúc.

### Data Models

#### GlobalNode

```csharp
public class GlobalNode
{
    public Guid Id { get; set; }              // Unique identifier
    public Guid MapId { get; set; }           // Map identifier
    public string? Name { get; set; }          // Node name
    public double X { get; set; }             // X coordinate
    public double Y { get; set; }             // Y coordinate
    public Orientation Orientation { get; set; } // FORWARD, BACKWARD, or NONE
    
    public double DistanceTo(GlobalNode other); // Calculate distance to another node
}
```

#### GlobalEdge

```csharp
public record GlobalEdge
{
    public Guid Id { get; set; }              // Unique identifier
    public Guid MapId { get; set; }           // Map identifier
    public Guid StartNodeId { get; set; }      // Start node ID
    public Guid EndNodeId { get; set; }       // End node ID
    public int Degree { get; set; }            // Curve degree (1, 2, or 3)
    public double ControlPoint1X { get; set; } // Control point 1 X (for Bezier)
    public double ControlPoint1Y { get; set; } // Control point 1 Y (for Bezier)
    public double ControlPoint2X { get; set; } // Control point 2 X (for Bezier)
    public double ControlPoint2Y { get; set; } // Control point 2 Y (for Bezier)
}
```

#### Orientation Enum

```csharp
public enum Orientation
{
    FORWARD,   // Di chuyển tiến
    BACKWARD,  // Di chuyển lùi
    NONE       // Không ràng buộc
}
```

## 💡 Ví dụ nâng cao

### Ví dụ 1: Path Planning với Error Handling

```csharp
try
{
    var factory = new PathPlannerFactory();
    var planner = factory.CreateDifferentialPlanner();
    
    planner.SetData(nodes, edges);
    
    var options = new PathPlannerOptions
    {
        LimitDistanceToEdge = 1.0,
        LimitDistanceToNode = 0.3,
        ResolutionSplit = 0.1,
        TimeOut = TimeSpan.FromSeconds(5),
        ChangeOrientationAngle = 89.0
    };
    planner.SetOptions(options);
    
    var (pathNodes, pathEdges) = planner.PathPlanning(
        x: currentX,
        y: currentY,
        theta: currentTheta,
        goalId: goalNodeId
    );
    
    Console.WriteLine($"Path found with {pathNodes.Length} nodes");
}
catch (Exception ex) when (ex.Message.Contains("does not exist"))
{
    Console.WriteLine($"Error: {ex.Message}");
    // Handle case when goal node doesn't exist or no path found
}
catch (TimeoutException)
{
    Console.WriteLine("Path planning timed out");
    // Handle timeout
}
catch (OperationCanceledException)
{
    Console.WriteLine("Path planning was cancelled");
    // Handle cancellation
}
```

### Ví dụ 2: Sử dụng với Async/Await

```csharp
public async Task<(GlobalNode[] Nodes, GlobalEdge[] Edges)> PlanPathAsync(
    IPathPlanner planner,
    double x, 
    double y, 
    double theta, 
    Guid goalId)
{
    using var cts = new CancellationTokenSource(TimeSpan.FromSeconds(10));
    
    return await Task.Run(() =>
    {
        return planner.PathPlanning(x, y, theta, goalId, cts.Token);
    }, cts.Token);
}

// Usage
var planner = factory.CreateDifferentialPlanner();
planner.SetData(nodes, edges);

var (pathNodes, pathEdges) = await PlanPathAsync(
    planner, 
    currentX, 
    currentY, 
    currentTheta, 
    goalNodeId
);
```

### Ví dụ 3: Path Planning với Direction Constraints

```csharp
// Robot cần bắt đầu di chuyển lùi
var (nodes, edges) = planner.PathPlanningWithStartDirection(
    x: 5.0,
    y: 5.0,
    theta: 180.0,
    goalId: goalNodeId,
    startDiretion: Orientation.BACKWARD
);

// Robot cần đến đích và quay mặt về phía trước
var (nodes2, edges2) = planner.PathPlanningWithFinalDirection(
    x: 5.0,
    y: 5.0,
    theta: 0.0,
    goalId: goalNodeId,
    goalDirection: Orientation.FORWARD
);

// Robot cần đến đích với góc cụ thể (90 độ)
var (nodes3, edges3) = planner.PathPlanningWithAngle(
    x: 5.0,
    y: 5.0,
    theta: 0.0,
    goalId: goalNodeId,
    goalAngle: 90.0
);
```

### Ví dụ 4: Xử lý kết quả Path

```csharp
var (pathNodes, pathEdges) = planner.PathPlanning(
    startNodeId: startNodeId,
    theta: currentTheta,
    goalId: goalNodeId
);

// Kiểm tra kết quả
if (pathNodes.Length == 0)
{
    Console.WriteLine("No path found");
    return;
}

// In thông tin path
Console.WriteLine($"Path contains {pathNodes.Length} nodes and {pathEdges.Length} edges");

for (int i = 0; i < pathNodes.Length; i++)
{
    var node = pathNodes[i];
    Console.WriteLine($"Node {i}: {node.Name} at ({node.X:F2}, {node.Y:F2}) " +
                     $"Orientation: {node.Orientation}");
    
    if (i < pathEdges.Length)
    {
        var edge = pathEdges[i];
        Console.WriteLine($"  Edge {i}: {edge.Id} (Degree: {edge.Degree})");
    }
}

// Tính tổng khoảng cách
double totalDistance = 0;
for (int i = 0; i < pathNodes.Length - 1; i++)
{
    totalDistance += pathNodes[i].DistanceTo(pathNodes[i + 1]);
}
Console.WriteLine($"Total path distance: {totalDistance:F2} meters");
```

## ✅ Best Practices

### 1. Chọn đúng Planner cho Robot Type

```csharp
// ✅ Đúng: Sử dụng ForkliftPlannerV2 cho forklift
var forkliftPlanner = factory.CreateForkliftPlannerV2();

// ❌ Sai: Không dùng DifferentialPlanner cho forklift
var wrongPlanner = factory.CreateDifferentialPlanner(); // Không phù hợp
```

### 2. Luôn Set Data trước khi Planning

```csharp
// ✅ Đúng
planner.SetData(nodes, edges);
var (nodes, edges) = planner.PathPlanning(...);

// ❌ Sai: Quên set data
var (nodes, edges) = planner.PathPlanning(...); // Sẽ lỗi
```

### 3. Sử dụng Node ID khi có thể

```csharp
// ✅ Tốt hơn: Sử dụng Node ID khi robot đã ở tại node
var (nodes, edges) = planner.PathPlanning(startNodeId, theta, goalId);

// ⚠️ Chấp nhận được: Sử dụng tọa độ khi robot không ở node
var (nodes, edges) = planner.PathPlanning(x, y, theta, goalId);
```

### 4. Cấu hình Timeout cho Operations dài

```csharp
var options = new PathPlannerOptions
{
    TimeOut = TimeSpan.FromSeconds(10) // Tránh hang
};
planner.SetOptions(options);
```

### 5. Xử lý Exceptions đúng cách

```csharp
try
{
    var (nodes, edges) = planner.PathPlanning(...);
}
catch (Exception ex) when (ex.Message.Contains("does not exist"))
{
    // Handle: Goal không tồn tại hoặc không tìm thấy đường đi
}
catch (TimeoutException)
{
    // Handle: Timeout
}
catch (OperationCanceledException)
{
    // Handle: Cancellation
}
```

### 6. Reuse Planner Instance

```csharp
// ✅ Tốt: Tạo một lần, dùng nhiều lần
var planner = factory.CreateDifferentialPlanner();
planner.SetData(nodes, edges);

for (int i = 0; i < 100; i++)
{
    var (pathNodes, pathEdges) = planner.PathPlanning(...);
    // Process path
}

// ❌ Không hiệu quả: Tạo mới mỗi lần
for (int i = 0; i < 100; i++)
{
    var planner = factory.CreateDifferentialPlanner(); // Không cần thiết
    planner.SetData(nodes, edges);
    var (pathNodes, pathEdges) = planner.PathPlanning(...);
}
```

## 🔧 Troubleshooting

### Vấn đề: "Destination does not exist in the map"

**Nguyên nhân:** Goal node ID không có trong danh sách nodes đã set.

**Giải pháp:**
```csharp
// Kiểm tra goal node có tồn tại không
var goalNode = nodes.FirstOrDefault(n => n.Id == goalId);
if (goalNode == null)
{
    throw new ArgumentException($"Goal node {goalId} not found in map");
}
```

### Vấn đề: "The path does not exist"

**Nguyên nhân:** Không có đường đi từ start đến goal (graph không liên thông).

**Giải pháp:**
- Kiểm tra edges có kết nối start và goal không
- Đảm bảo graph liên thông
- Kiểm tra `LimitDistanceToEdge` và `LimitDistanceToNode` có quá nhỏ không

### Vấn đề: Timeout thường xuyên

**Nguyên nhân:** Graph quá lớn hoặc phức tạp.

**Giải pháp:**
```csharp
// Tăng timeout
var options = new PathPlannerOptions
{
    TimeOut = TimeSpan.FromSeconds(30) // Tăng từ 10s lên 30s
};
planner.SetOptions(options);
```

### Vấn đề: Path không mượt

**Nguyên nhân:** `ResolutionSplit` quá lớn.

**Giải pháp:**
```csharp
// Giảm ResolutionSplit để có nhiều điểm hơn
var options = new PathPlannerOptions
{
    ResolutionSplit = 0.05 // Giảm từ 0.1 xuống 0.05
};
planner.SetOptions(options);
```

### Vấn đề: Robot không đổi hướng đúng

**Nguyên nhân:** `ChangeOrientationAngle` không phù hợp.

**Giải pháp:**
```csharp
// Điều chỉnh góc đổi hướng
var options = new PathPlannerOptions
{
    ChangeOrientationAngle = 85.0 // Thử các giá trị khác nhau
};
planner.SetOptions(options);
```

## 📝 Lưu ý

- **Thread Safety**: Planner instances không thread-safe. Mỗi thread nên có planner riêng.
- **Memory**: Planner lưu toàn bộ nodes và edges trong memory. Với map lớn, cần xem xét memory usage.
- **Performance**: Path planning với Node ID nhanh hơn so với tọa độ vì không cần tìm nearest node/edge.

## 🤝 Đóng góp

Nếu bạn phát hiện bug hoặc có đề xuất cải thiện, vui lòng tạo issue hoặc pull request.

## 📄 License

[Thêm thông tin license nếu có]

---

**Version**: 1.0  
**Last Updated**: 2024