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
• Cài đặt
• Bắt đầu nhanh
• Hướng dẫn sử dụng
• Các loại Path Planner
• Configuration Options
• API Reference
• Ví dụ nâng cao
• Best Practices
• 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:

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

Using Statements

using RobotNet10.GlobalPathPlanner;
using RobotNet10.GlobalPathPlanner.Model;

🚀 Bắt đầu nhanh

Ví dụ cơ bản

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

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

// 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 độ)

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

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

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

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

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

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ụ:

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ụ:

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ụ:

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ụ:

var planner = factory.CreateOmniDrivePlanner();

⚙️ Configuration Options

PathPlannerOptions

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

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

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 độ)

(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)

(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

(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

(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

(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

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

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

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

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

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

// 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

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

// ✅ Đú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

// ✅ Đú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ể

// ✅ 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

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

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

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

// ✅ 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:

// 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:

// 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:

// 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:

// Đ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