Initial commit
This commit is contained in:
@@ -0,0 +1,85 @@
|
||||
{
|
||||
"headerId": 1,
|
||||
"timestamp": "2026-02-28T16:01:25.308Z",
|
||||
"version": "0.0.1",
|
||||
"manufacturer": "phenikaaX",
|
||||
"serialNumber": "T800Fake",
|
||||
"orderId": "c92ec33e-ec04-4521-8255-af8ae46b7f31",
|
||||
"orderUpdateId": 1,
|
||||
"zoneSetId": "maze",
|
||||
"nodes": [
|
||||
{
|
||||
"nodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"sequenceId": 0,
|
||||
"nodeDescription": "RobotCurrentNode",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 2.5,
|
||||
"y": 0.4,
|
||||
"theta": -0.04770435896493348,
|
||||
"allowedDeviationXY": 0,
|
||||
"allowedDeviationTheta": 0,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"sequenceId": 1,
|
||||
"nodeDescription": "Node172",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 4.5,
|
||||
"y": 0.4,
|
||||
"theta": -0.04770435896493223,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
],
|
||||
"edges": [
|
||||
{
|
||||
"edgeId": "522212fa-74d5-4f00-9a00-23e092593bcc",
|
||||
"sequenceId": 0,
|
||||
"edgeDescription": "86312653-08b2-4556-bb84-674805905bed - 3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"released": true,
|
||||
"startNodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"endNodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"maxSpeed": 0.5,
|
||||
"maxHeight": 0,
|
||||
"minHeight": 0,
|
||||
"orientation": 0,
|
||||
"orientationType": "",
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"length": 0,
|
||||
"trajectory": {
|
||||
"degree": 1,
|
||||
"knotVector": [
|
||||
0,
|
||||
0,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"controlPoints": [
|
||||
{
|
||||
"x": 3.5,
|
||||
"y": 0.4,
|
||||
"weight": 1
|
||||
},
|
||||
{
|
||||
"x": 4.5,
|
||||
"y": 0.4,
|
||||
"weight": 1
|
||||
}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,362 @@
|
||||
|
||||
# Lấy pose hiện tại
|
||||
```bash
|
||||
curl -k -X GET https://localhost:7002/api/navigation/pose/current2d
|
||||
```
|
||||
# Di chuyển đến vị trí (1.0, 2.0) với yaw = 0
|
||||
```bash
|
||||
# Move to goal
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{"x": 1.0, "y": 2.0, "yaw": 0.0, "frame_id": "map"}'
|
||||
```
|
||||
|
||||
# Di chuyển đến vị trí với order (navigation_move_to_order)
|
||||
#
|
||||
# ⚠️ LƯU Ý QUAN TRỌNG:
|
||||
# - order_handle phải là một pointer hợp lệ đến Order object được tạo bởi navigation system.
|
||||
# - KHÔNG thể sử dụng số ngẫu nhiên hoặc giá trị test (như 0x12345678) - phải là pointer thực sự từ C++ code.
|
||||
# - Nếu sử dụng order_handle không hợp lệ, server có thể crash hoặc trả về lỗi SSL.
|
||||
# - order_handle có thể là số decimal hoặc hex string (0x...)
|
||||
# - Giá trị phải > 0x1000 để được coi là pointer hợp lệ
|
||||
# - Để có order_handle hợp lệ, bạn cần tạo Order object từ navigation system trước
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"order_handle": "0x12345678",
|
||||
"x": 1.0,
|
||||
"y": 2.0,
|
||||
"z": 0.0,
|
||||
"qx": 0.0,
|
||||
"qy": 0.0,
|
||||
"qz": 0.0,
|
||||
"qw": 1.0,
|
||||
"frame_id": "map",
|
||||
"xy_tolerance": 0.1,
|
||||
"yaw_tolerance": 0.1
|
||||
}'
|
||||
```
|
||||
|
||||
# Lưu ý: JSON phải được format đúng. Nếu gặp lỗi, có thể sử dụng format một dòng:
|
||||
# ⚠️ LƯU Ý: order_handle "0x12345678" trong ví dụ này chỉ là placeholder - bạn PHẢI thay bằng order_handle thực sự từ navigation system!
|
||||
```bash
|
||||
# Ví dụ với order_handle thực (thay 0x12345678 bằng giá trị thực):
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order -H "Content-Type: application/json" -d '{"order_handle":"0x12345678","x":1.0,"y":2.0,"z":0.0,"qx":0.0,"qy":0.0,"qz":0.0,"qw":1.0,"frame_id":"map","xy_tolerance":0.1,"yaw_tolerance":0.1}'
|
||||
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to -H "Content-Type: application/json" -d '{"x": 1.0, "y": 2.0, "yaw": 0.0, "frame_id": "map"}'
|
||||
```
|
||||
|
||||
# Hoặc sử dụng yaw thay vì quaternion:
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"order_handle": 305419896,
|
||||
"x": 1.0,
|
||||
"y": 2.0,
|
||||
"yaw": 0.0,
|
||||
"frame_id": "map",
|
||||
"xy_tolerance": 0.1,
|
||||
"yaw_tolerance": 0.1
|
||||
}'
|
||||
```
|
||||
|
||||
# Cấu trúc OrderHandle trong C#:
|
||||
# - OrderHandle: Wrapper class cho IntPtr, đại diện cho pointer đến Order object
|
||||
# - OrderData: C# class chứa dữ liệu Order (headerId, orderId, nodes, edges, etc.)
|
||||
#
|
||||
# Hiện tại, order_data chưa được hỗ trợ (cần C API function để tạo OrderHandle từ OrderData).
|
||||
# Vui lòng sử dụng order_handle (IntPtr) từ navigation system.
|
||||
|
||||
# Di chuyển thẳng theo hướng hiện tại (distance mét). Bắt buộc có trường "distance".
|
||||
```bash
|
||||
# Đi thẳng 2.0 mét
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_straight_to \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{"distance": 1.0}'
|
||||
```
|
||||
|
||||
# Ví dụ khác:
|
||||
```bash
|
||||
# Đi thẳng 1.5 mét
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_straight_to \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{"distance": 1.5}'
|
||||
|
||||
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_straight_to -H "Content-Type: application/json" -d '{"distance": 2.0}'
|
||||
```
|
||||
|
||||
# Xoay đến yaw = 1.57 (90 độ)
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/rotate_to \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{"yaw": 3.14}'
|
||||
```
|
||||
# Tạm dừng
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/pause
|
||||
```
|
||||
# Tiếp tục
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/resume
|
||||
```
|
||||
# Hủy goal
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/pause
|
||||
```
|
||||
# Lấy feedback
|
||||
```bash
|
||||
curl -k -X GET https://localhost:7002/api/navigation/feedback
|
||||
```
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/twist/linear \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{"x": 0.5, "y": 0.0, "z": 0.0}'
|
||||
```
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order_data \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"order": {
|
||||
"orderId": "order_001",
|
||||
"orderUpdateId": 1,
|
||||
"nodes": [{"nodeId": "node1", "sequenceId": 1, "released": true, "nodePosition": {"x": 0.0, "y": 0.0, "mapId": "map"}}],
|
||||
"edges": [{"edgeId": "edge1", "sequenceId": 2, "startNodeId": "node1", "endNodeId": "node2", "released": true}]
|
||||
},
|
||||
"x": 13.05,
|
||||
"y": 3.41,
|
||||
"z": 0.0,
|
||||
"yaw": 1.57
|
||||
}'
|
||||
```
|
||||
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order_data \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"x": 7.0,
|
||||
"y": 2.0,
|
||||
"yaw": 1.57,
|
||||
"frame_id": "map",
|
||||
"xy_tolerance": 0.3,
|
||||
"yaw_tolerance": 50,
|
||||
"order": {
|
||||
"headerId": 1,
|
||||
"timestamp": "2026-02-28T16:01:25.308Z",
|
||||
"version": "0.0.1",
|
||||
"manufacturer": "phenikaaX",
|
||||
"serialNumber": "T800Fake",
|
||||
"orderId": "c92ec33e-ec04-4521-8255-af8ae46b7f31",
|
||||
"orderUpdateId": 1,
|
||||
"zoneSetId": "maze",
|
||||
"nodes": [
|
||||
{
|
||||
"nodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"sequenceId": 0,
|
||||
"nodeDescription": "Node0",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 2.5,
|
||||
"y": 1.0,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0,
|
||||
"allowedDeviationTheta": 0,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"sequenceId": 1,
|
||||
"nodeDescription": "Node1",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 6.0,
|
||||
"y": 1.0,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "1231321231231-fdfdf-fdfdsafa",
|
||||
"sequenceId": 2,
|
||||
"nodeDescription": "Node2",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 7.0,
|
||||
"y": 2.0,
|
||||
"theta": 1.57,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
],
|
||||
"edges": [
|
||||
{
|
||||
"edgeId": "522212fa-74d5-4f00-9a00-23e092593bcc",
|
||||
"sequenceId": 0,
|
||||
"edgeDescription": "86312653-08b2-4556-bb84-674805905bed - 3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"released": true,
|
||||
"startNodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"endNodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"maxSpeed": 0.5,
|
||||
"maxHeight": 0,
|
||||
"minHeight": 0,
|
||||
"orientation": 0,
|
||||
"orientationType": "",
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"length": 0,
|
||||
"trajectory": {
|
||||
"degree": 1,
|
||||
"knotVector": [0, 0, 1, 1],
|
||||
"controlPoints": [
|
||||
{
|
||||
"x": 2.5,
|
||||
"y": 1.0,
|
||||
"weight": 1
|
||||
},
|
||||
{
|
||||
"x": 6.0,
|
||||
"y": 1.0,
|
||||
"weight": 1
|
||||
}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"edgeId": "13312fds-fdsfsdaaf",
|
||||
"sequenceId": 1,
|
||||
"edgeDescription": "3726205f-0140-454d-21a8-08de5340bfbf - 1231321231231-fdfdf-fdfdsafa",
|
||||
"released": true,
|
||||
"startNodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"endNodeId": "1231321231231-fdfdf-fdfdsafa",
|
||||
"maxSpeed": 0.5,
|
||||
"maxHeight": 0,
|
||||
"minHeight": 0,
|
||||
"orientation": 0,
|
||||
"orientationType": "",
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"length": 0,
|
||||
"trajectory": {
|
||||
"degree": 2,
|
||||
"knotVector": [0, 0, 0, 1, 1, 1],
|
||||
"controlPoints": [
|
||||
{"x": 6.0, "y": 1.0, "weight": 1.0},
|
||||
{"x": 7.0, "y": 0.5, "weight": 0.707},
|
||||
{"x": 7.0, "y": 2.0, "weight": 1.0}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
]
|
||||
}
|
||||
}'
|
||||
```
|
||||
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order_data \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"x": 2.5,
|
||||
"y": 0.4,
|
||||
"yaw": 0.0,
|
||||
"frame_id": "map",
|
||||
"xy_tolerance": 0.3,
|
||||
"yaw_tolerance": 50,
|
||||
"order": {
|
||||
"headerId": 1,
|
||||
"timestamp": "2026-02-28T16:01:25.308Z",
|
||||
"version": "0.0.1",
|
||||
"manufacturer": "phenikaaX",
|
||||
"serialNumber": "T800Fake",
|
||||
"orderId": "c92ec33e-ec04-4521-8255-af8ae46b7f31",
|
||||
"orderUpdateId": 1,
|
||||
"zoneSetId": "maze",
|
||||
"nodes": [
|
||||
{
|
||||
"nodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"sequenceId": 0,
|
||||
"nodeDescription": "RobotCurrentNode",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 5.5,
|
||||
"y": 0.4,
|
||||
"theta": -0.0,
|
||||
"allowedDeviationXY": 0,
|
||||
"allowedDeviationTheta": 0,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"sequenceId": 1,
|
||||
"nodeDescription": "Node172",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 2.5,
|
||||
"y": 0.4,
|
||||
"theta": -0.0,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
],
|
||||
"edges": [
|
||||
{
|
||||
"edgeId": "522212fa-74d5-4f00-9a00-23e092593bcc",
|
||||
"sequenceId": 0,
|
||||
"edgeDescription": "86312653-08b2-4556-bb84-674805905bed - 3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"released": true,
|
||||
"startNodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"endNodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"maxSpeed": 0.5,
|
||||
"maxHeight": 0,
|
||||
"minHeight": 0,
|
||||
"orientation": 0,
|
||||
"orientationType": "",
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"length": 0,
|
||||
"trajectory": {
|
||||
"degree": 1,
|
||||
"knotVector": [0, 0, 1, 1],
|
||||
"controlPoints": [
|
||||
{
|
||||
"x": 4.5,
|
||||
"y": 0.4,
|
||||
"weight": 1
|
||||
},
|
||||
{
|
||||
"x": 3.5,
|
||||
"y": 0.4,
|
||||
"weight": 1
|
||||
}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
]
|
||||
}
|
||||
}'
|
||||
```
|
||||
@@ -0,0 +1,859 @@
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
using System.Text.Json;
|
||||
using RobotNet10.RobotApp.Motion;
|
||||
using RobotNet10.RobotApp.Xloc;
|
||||
|
||||
namespace RobotNet10.RobotApp.Navigation;
|
||||
|
||||
public static class NavigationApiEndpoints
|
||||
{
|
||||
// Navigation Control API
|
||||
public static IEndpointRouteBuilder MapNavigationApiEndpoints(this IEndpointRouteBuilder app)
|
||||
{
|
||||
var navApi = app.MapGroup("/api/navigation").DisableAntiforgery();
|
||||
|
||||
// Navigation control - Move to goal
|
||||
navApi.MapPost("/move_to", async (HttpRequest request, [FromServices] NavigationIntegrationService service) => {
|
||||
try
|
||||
{
|
||||
var body = await request.ReadFromJsonAsync<JsonElement>();
|
||||
if (body.ValueKind != JsonValueKind.Object)
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
||||
|
||||
if (!body.TryGetProperty("x", out var xProp) || !body.TryGetProperty("y", out var yProp))
|
||||
return Results.BadRequest(new { status = "error", message = "x and y are required" });
|
||||
|
||||
double x = xProp.GetDouble();
|
||||
double y = yProp.GetDouble();
|
||||
double z = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
|
||||
|
||||
// Orientation (quaternion or euler angles)
|
||||
double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
|
||||
if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) &&
|
||||
body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
|
||||
{
|
||||
qx = qxProp.GetDouble();
|
||||
qy = qyProp.GetDouble();
|
||||
qz = qzProp.GetDouble();
|
||||
qw = qwProp.GetDouble();
|
||||
}
|
||||
else if (body.TryGetProperty("yaw", out var yawProp))
|
||||
{
|
||||
// Convert yaw to quaternion
|
||||
double yaw = yawProp.GetDouble();
|
||||
qw = Math.Cos(yaw / 2.0);
|
||||
qz = Math.Sin(yaw / 2.0);
|
||||
}
|
||||
|
||||
string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
|
||||
double xyTolerance = body.TryGetProperty("xy_tolerance", out var xyTolProp) ? xyTolProp.GetDouble() : 0.1;
|
||||
double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.1;
|
||||
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
bool result = await Task.Run(() => service.MoveTo(x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
|
||||
return result
|
||||
? Results.Ok(new { status = "success", message = "Move to goal sent", goal = new { x, y, z, qx, qy, qz, qw, frameId } })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to send move to goal" });
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
|
||||
}
|
||||
});
|
||||
|
||||
// Navigation control - Move to goal with order
|
||||
navApi.MapPost("/move_to_order", async (HttpRequest request, [FromServices] NavigationIntegrationService service, [FromServices] ILogger<Program> logger) => {
|
||||
try
|
||||
{
|
||||
logger.LogInformation("Received move_to_order request");
|
||||
var body = await request.ReadFromJsonAsync<JsonElement>();
|
||||
if (body.ValueKind != JsonValueKind.Object)
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
||||
|
||||
if (!body.TryGetProperty("x", out var xProp) || !body.TryGetProperty("y", out var yProp))
|
||||
return Results.BadRequest(new { status = "error", message = "x and y are required" });
|
||||
|
||||
// Order handle - can be provided as:
|
||||
// 1. order_handle: IntPtr value (number or hex string)
|
||||
// 2. order_data: OrderData object (will be converted to OrderHandle)
|
||||
IntPtr orderHandle = IntPtr.Zero;
|
||||
OrderHandle? orderHandleWrapper = null;
|
||||
|
||||
if (body.TryGetProperty("order_handle", out var orderHandleProp))
|
||||
{
|
||||
if (orderHandleProp.ValueKind == JsonValueKind.String)
|
||||
{
|
||||
string orderHandleStr = orderHandleProp.GetString() ?? "0";
|
||||
// Try parse as hex if starts with 0x, otherwise as decimal
|
||||
if (orderHandleStr.StartsWith("0x", StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
orderHandle = new IntPtr(Convert.ToInt64(orderHandleStr, 16));
|
||||
}
|
||||
else
|
||||
{
|
||||
orderHandle = new IntPtr(Convert.ToInt64(orderHandleStr));
|
||||
}
|
||||
}
|
||||
else if (orderHandleProp.ValueKind == JsonValueKind.Number)
|
||||
{
|
||||
orderHandle = new IntPtr(orderHandleProp.GetInt64());
|
||||
}
|
||||
|
||||
// Validate order handle is a reasonable pointer value (not too small)
|
||||
// Typical valid pointers on 64-bit systems are > 0x1000
|
||||
if (orderHandle != IntPtr.Zero && orderHandle.ToInt64() < 0x1000)
|
||||
{
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = $"Invalid order_handle value: {orderHandle.ToInt64()}. Order handle must be a valid pointer to an Order object created by the navigation system. Use a value > 0x1000 or provide order_data instead.",
|
||||
order_handle = orderHandle.ToInt64()
|
||||
});
|
||||
}
|
||||
|
||||
if (orderHandle != IntPtr.Zero)
|
||||
{
|
||||
orderHandleWrapper = new OrderHandle(orderHandle);
|
||||
}
|
||||
}
|
||||
else if (body.TryGetProperty("order_data", out var orderDataProp))
|
||||
{
|
||||
// Try to deserialize OrderData from JSON
|
||||
try
|
||||
{
|
||||
var orderData = JsonSerializer.Deserialize<OrderData>(orderDataProp.GetRawText());
|
||||
if (orderData != null)
|
||||
{
|
||||
// Note: Currently, we cannot create OrderHandle from OrderData without C API support
|
||||
// For now, return error suggesting to use order_handle
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = "order_data is provided but OrderHandle creation from OrderData is not yet supported. Please use order_handle (IntPtr) instead, or implement C API function to create OrderHandle from Order data."
|
||||
});
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = $"Failed to parse order_data: {ex.Message}"
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
if (orderHandle == IntPtr.Zero && (orderHandleWrapper == null || !orderHandleWrapper.IsValid))
|
||||
return Results.BadRequest(new { status = "error", message = "Either order_handle or order_data is required" });
|
||||
|
||||
double x = xProp.GetDouble();
|
||||
double y = yProp.GetDouble();
|
||||
double z = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
|
||||
|
||||
// Orientation (quaternion or euler angles)
|
||||
double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
|
||||
if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) &&
|
||||
body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
|
||||
{
|
||||
qx = qxProp.GetDouble();
|
||||
qy = qyProp.GetDouble();
|
||||
qz = qzProp.GetDouble();
|
||||
qw = qwProp.GetDouble();
|
||||
}
|
||||
else if (body.TryGetProperty("yaw", out var yawProp))
|
||||
{
|
||||
// Convert yaw to quaternion
|
||||
double yaw = yawProp.GetDouble();
|
||||
qw = Math.Cos(yaw / 2.0);
|
||||
qz = Math.Sin(yaw / 2.0);
|
||||
}
|
||||
|
||||
string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
|
||||
double xyTolerance = body.TryGetProperty("xy_tolerance", out var xyTolProp) ? xyTolProp.GetDouble() : 0.1;
|
||||
double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.1;
|
||||
|
||||
try
|
||||
{
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
bool result;
|
||||
long orderHandleValue = orderHandle.ToInt64();
|
||||
|
||||
if (orderHandleWrapper != null && orderHandleWrapper.IsValid)
|
||||
{
|
||||
try
|
||||
{
|
||||
logger.LogInformation("Calling MoveToOrder with OrderHandle wrapper: {OrderHandle}, goal: ({X}, {Y}, {Z}), frame: {FrameId}",
|
||||
orderHandleWrapper.Handle.ToInt64(), x, y, z, frameId);
|
||||
result = await Task.Run(() => service.MoveToOrder(orderHandleWrapper, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
|
||||
orderHandleValue = orderHandleWrapper.Handle.ToInt64();
|
||||
logger.LogInformation("MoveToOrder completed with result: {Result}, order_handle: {OrderHandle}", result, orderHandleValue);
|
||||
}
|
||||
catch (AccessViolationException avex)
|
||||
{
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = $"Access violation: Order handle {orderHandleValue} (0x{orderHandleValue:X}) is not a valid pointer to an Order object. The order handle must be created by the navigation system.",
|
||||
order_handle = orderHandleValue,
|
||||
error_type = "AccessViolationException"
|
||||
});
|
||||
}
|
||||
catch (ArgumentException aex)
|
||||
{
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = aex.Message,
|
||||
order_handle = orderHandleValue,
|
||||
error_type = "ArgumentException"
|
||||
});
|
||||
}
|
||||
}
|
||||
else if (orderHandle != IntPtr.Zero)
|
||||
{
|
||||
try
|
||||
{
|
||||
logger.LogInformation("Calling MoveToOrder with IntPtr: {OrderHandle}, goal: ({X}, {Y}, {Z}), frame: {FrameId}",
|
||||
orderHandleValue, x, y, z, frameId);
|
||||
result = await Task.Run(() => service.MoveToOrder(orderHandle, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
|
||||
logger.LogInformation("MoveToOrder completed with result: {Result}, order_handle: {OrderHandle}", result, orderHandleValue);
|
||||
}
|
||||
catch (AccessViolationException avex)
|
||||
{
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = $"Access violation: Order handle {orderHandleValue} (0x{orderHandleValue:X}) is not a valid pointer to an Order object. The order handle must be created by the navigation system.",
|
||||
order_handle = orderHandleValue,
|
||||
error_type = "AccessViolationException"
|
||||
});
|
||||
}
|
||||
catch (ArgumentException aex)
|
||||
{
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = aex.Message,
|
||||
order_handle = orderHandleValue,
|
||||
error_type = "ArgumentException"
|
||||
});
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid order handle" });
|
||||
}
|
||||
|
||||
return result
|
||||
? Results.Ok(new { status = "success", message = "Move to goal with order sent", goal = new { x, y, z, qx, qy, qz, qw, frameId }, order_handle = orderHandleValue })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to send move to goal with order. Order handle may be invalid or navigation system rejected the command.", order_handle = orderHandleValue });
|
||||
}
|
||||
catch (AccessViolationException avex)
|
||||
{
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = $"Access violation: Order handle {orderHandle.ToInt64()} (0x{orderHandle.ToInt64():X}) is not a valid pointer to an Order object.",
|
||||
order_handle = orderHandle.ToInt64(),
|
||||
error_type = "AccessViolationException"
|
||||
});
|
||||
}
|
||||
catch (ArgumentException ex)
|
||||
{
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = ex.Message,
|
||||
order_handle = orderHandle.ToInt64(),
|
||||
error_type = "ArgumentException"
|
||||
});
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = $"Internal error: {ex.Message}",
|
||||
order_handle = orderHandle.ToInt64(),
|
||||
error_type = ex.GetType().Name,
|
||||
stack_trace = ex.StackTrace
|
||||
});
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
|
||||
}
|
||||
});
|
||||
|
||||
// Navigation control - Move to goal with OrderData (JSON)
|
||||
navApi.MapPost("/move_to_order_data", async (HttpRequest request, [FromServices] NavigationIntegrationService service, [FromServices] ILogger<Program> logger) => {
|
||||
try
|
||||
{
|
||||
logger.LogInformation("Received move_to_order_data request");
|
||||
var body = await request.ReadFromJsonAsync<JsonElement>();
|
||||
if (body.ValueKind != JsonValueKind.Object)
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
||||
|
||||
// Parse goal position
|
||||
if (!body.TryGetProperty("x", out var xProp) || !body.TryGetProperty("y", out var yProp))
|
||||
return Results.BadRequest(new { status = "error", message = "x and y are required" });
|
||||
|
||||
double x = xProp.GetDouble();
|
||||
double y = yProp.GetDouble();
|
||||
double z = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
|
||||
|
||||
// Orientation (quaternion or euler angles)
|
||||
double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
|
||||
if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) &&
|
||||
body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
|
||||
{
|
||||
qx = qxProp.GetDouble();
|
||||
qy = qyProp.GetDouble();
|
||||
qz = qzProp.GetDouble();
|
||||
qw = qwProp.GetDouble();
|
||||
}
|
||||
else if (body.TryGetProperty("yaw", out var yawProp))
|
||||
{
|
||||
// Convert yaw to quaternion
|
||||
double yaw = yawProp.GetDouble();
|
||||
qw = Math.Cos(yaw / 2.0);
|
||||
qz = Math.Sin(yaw / 2.0);
|
||||
}
|
||||
|
||||
string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
|
||||
double xyTolerance = body.TryGetProperty("xy_tolerance", out var xyTolProp) ? xyTolProp.GetDouble() : 0.1;
|
||||
double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.1;
|
||||
|
||||
// Parse OrderData from "order" field
|
||||
if (!body.TryGetProperty("order", out var orderProp))
|
||||
return Results.BadRequest(new { status = "error", message = "order is required" });
|
||||
|
||||
OrderData? orderData = null;
|
||||
try
|
||||
{
|
||||
orderData = JsonSerializer.Deserialize<OrderData>(orderProp.GetRawText(), new JsonSerializerOptions
|
||||
{
|
||||
PropertyNameCaseInsensitive = true
|
||||
});
|
||||
|
||||
if (orderData == null)
|
||||
return Results.BadRequest(new { status = "error", message = "Failed to parse order data" });
|
||||
|
||||
logger.LogInformation("Parsed OrderData: OrderId={OrderId}, Nodes={NodeCount}, Edges={EdgeCount}",
|
||||
orderData.OrderId, orderData.Nodes?.Count ?? 0, orderData.Edges?.Count ?? 0);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
logger.LogError(ex, "Failed to deserialize OrderData");
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = $"Failed to parse order: {ex.Message}"
|
||||
});
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
bool result = await Task.Run(() => service.MoveToOrder(orderData, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
|
||||
|
||||
if (result)
|
||||
{
|
||||
logger.LogInformation("MoveToOrder successfully sent with OrderData: {OrderId}", orderData.OrderId);
|
||||
return Results.Ok(new {
|
||||
status = "success",
|
||||
message = "Move to goal with order sent",
|
||||
goal = new { x, y, z, qx, qy, qz, qw, frameId },
|
||||
order = new {
|
||||
orderId = orderData.OrderId,
|
||||
orderUpdateId = orderData.OrderUpdateId,
|
||||
nodeCount = orderData.Nodes?.Count ?? 0,
|
||||
edgeCount = orderData.Edges?.Count ?? 0
|
||||
}
|
||||
});
|
||||
}
|
||||
else
|
||||
{
|
||||
logger.LogWarning("MoveToOrder failed for OrderData: {OrderId}", orderData.OrderId);
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = "Failed to send move to goal with order. Navigation system rejected the command.",
|
||||
orderId = orderData.OrderId
|
||||
});
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
logger.LogError(ex, "Error in MoveToOrder with OrderData: {OrderId}", orderData?.OrderId ?? "null");
|
||||
return Results.BadRequest(new {
|
||||
status = "error",
|
||||
message = $"Internal error: {ex.Message}",
|
||||
error_type = ex.GetType().Name
|
||||
});
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
logger.LogError(ex, "Failed to parse move_to_order_data request");
|
||||
return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
|
||||
}
|
||||
});
|
||||
|
||||
// Navigation control - Dock to marker
|
||||
navApi.MapPost("/dock_to", async (HttpRequest request, [FromServices] NavigationIntegrationService service) => {
|
||||
try
|
||||
{
|
||||
var body = await request.ReadFromJsonAsync<JsonElement>();
|
||||
if (body.ValueKind != JsonValueKind.Object)
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
||||
|
||||
if (!body.TryGetProperty("marker", out var markerProp))
|
||||
return Results.BadRequest(new { status = "error", message = "marker is required" });
|
||||
|
||||
string marker = markerProp.GetString() ?? "";
|
||||
if (string.IsNullOrEmpty(marker))
|
||||
return Results.BadRequest(new { status = "error", message = "marker cannot be empty" });
|
||||
|
||||
if (!body.TryGetProperty("x", out var xProp) || !body.TryGetProperty("y", out var yProp))
|
||||
return Results.BadRequest(new { status = "error", message = "x and y are required" });
|
||||
|
||||
double x = xProp.GetDouble();
|
||||
double y = yProp.GetDouble();
|
||||
double z = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
|
||||
|
||||
double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
|
||||
if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) &&
|
||||
body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
|
||||
{
|
||||
qx = qxProp.GetDouble();
|
||||
qy = qyProp.GetDouble();
|
||||
qz = qzProp.GetDouble();
|
||||
qw = qwProp.GetDouble();
|
||||
}
|
||||
else if (body.TryGetProperty("yaw", out var yawProp))
|
||||
{
|
||||
double yaw = yawProp.GetDouble();
|
||||
qw = Math.Cos(yaw / 2.0);
|
||||
qz = Math.Sin(yaw / 2.0);
|
||||
}
|
||||
|
||||
string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
|
||||
double xyTolerance = body.TryGetProperty("xy_tolerance", out var xyTolProp) ? xyTolProp.GetDouble() : 0.05;
|
||||
double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.05;
|
||||
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
bool result = await Task.Run(() => service.DockTo(marker, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
|
||||
return result
|
||||
? Results.Ok(new { status = "success", message = "Dock to goal sent", marker, goal = new { x, y, z, qx, qy, qz, qw, frameId } })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to send dock to goal" });
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
|
||||
}
|
||||
});
|
||||
|
||||
// Navigation control - Move straight by distance (meters) in current direction
|
||||
navApi.MapPost("/move_straight_to", async (HttpRequest request, [FromServices] NavigationIntegrationService service) => {
|
||||
try
|
||||
{
|
||||
var body = await request.ReadFromJsonAsync<JsonElement>();
|
||||
if (body.ValueKind != JsonValueKind.Object)
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
||||
|
||||
if (!body.TryGetProperty("distance", out var distProp))
|
||||
return Results.BadRequest(new { status = "error", message = "distance is required" });
|
||||
double distance = distProp.GetDouble();
|
||||
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
bool result = await Task.Run(() => service.MoveStraightTo(distance));
|
||||
return result
|
||||
? Results.Ok(new { status = "success", message = "Move straight to goal sent", distance = distance })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to send move straight to goal" });
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
|
||||
}
|
||||
});
|
||||
|
||||
// Navigation control - Rotate to orientation
|
||||
navApi.MapPost("/rotate_to", async (HttpRequest request, [FromServices] NavigationIntegrationService service) => {
|
||||
try
|
||||
{
|
||||
var body = await request.ReadFromJsonAsync<JsonElement>();
|
||||
if (body.ValueKind != JsonValueKind.Object)
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
||||
|
||||
// For rotation, we need current position and target orientation
|
||||
double x = 0.0, y = 0.0, z = 0.0;
|
||||
if (body.TryGetProperty("x", out var xProp)) x = xProp.GetDouble();
|
||||
if (body.TryGetProperty("y", out var yProp)) y = yProp.GetDouble();
|
||||
if (body.TryGetProperty("z", out var zProp)) z = zProp.GetDouble();
|
||||
|
||||
double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
|
||||
if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) &&
|
||||
body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
|
||||
{
|
||||
qx = qxProp.GetDouble();
|
||||
qy = qyProp.GetDouble();
|
||||
qz = qzProp.GetDouble();
|
||||
qw = qwProp.GetDouble();
|
||||
}
|
||||
else if (body.TryGetProperty("yaw", out var yawProp))
|
||||
{
|
||||
double yaw = yawProp.GetDouble();
|
||||
qw = Math.Cos(yaw / 2.0);
|
||||
qz = Math.Sin(yaw / 2.0);
|
||||
}
|
||||
else
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = "yaw or quaternion (qx, qy, qz, qw) is required" });
|
||||
}
|
||||
|
||||
string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
|
||||
double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.1;
|
||||
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
bool result = await Task.Run(() => service.RotateTo(x, y, z, qx, qy, qz, qw, frameId, yawTolerance));
|
||||
return result
|
||||
? Results.Ok(new { status = "success", message = "Rotate to goal sent", goal = new { x, y, z, qx, qy, qz, qw, frameId } })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to send rotate to goal" });
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
|
||||
}
|
||||
});
|
||||
|
||||
// Navigation control - Pause
|
||||
navApi.MapPost("/pause", async ([FromServices] NavigationIntegrationService service) => {
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
await Task.Run(() => service.Pause());
|
||||
return Results.Ok(new { status = "success", message = "Navigation paused" });
|
||||
});
|
||||
|
||||
// Navigation control - Resume
|
||||
navApi.MapPost("/resume", async ([FromServices] NavigationIntegrationService service) => {
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
await Task.Run(() => service.Resume());
|
||||
return Results.Ok(new { status = "success", message = "Navigation resumed" });
|
||||
});
|
||||
|
||||
// Navigation control - Cancel
|
||||
navApi.MapPost("/cancel", async ([FromServices] NavigationIntegrationService service) => {
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
await Task.Run(() => service.Cancel());
|
||||
return Results.Ok(new { status = "success", message = "Navigation cancelled" });
|
||||
});
|
||||
|
||||
// Navigation control - Set linear twist
|
||||
navApi.MapPost("/twist/linear", async (HttpRequest request, [FromServices] NavigationIntegrationService service) => {
|
||||
try
|
||||
{
|
||||
var body = await request.ReadFromJsonAsync<JsonElement>();
|
||||
if (body.ValueKind != JsonValueKind.Object)
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
||||
|
||||
double linearX = body.TryGetProperty("x", out var xProp) ? xProp.GetDouble() : 0.0;
|
||||
double linearY = body.TryGetProperty("y", out var yProp) ? yProp.GetDouble() : 0.0;
|
||||
double linearZ = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
|
||||
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
bool result = await Task.Run(() => service.SetTwistLinear(linearX, linearY, linearZ));
|
||||
return result
|
||||
? Results.Ok(new { status = "success", message = "Linear twist set", twist = new { x = linearX, y = linearY, z = linearZ } })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to set linear twist" });
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
|
||||
}
|
||||
});
|
||||
|
||||
// Navigation control - Set angular twist
|
||||
navApi.MapPost("/twist/angular", async (HttpRequest request, [FromServices] NavigationIntegrationService service) => {
|
||||
try
|
||||
{
|
||||
var body = await request.ReadFromJsonAsync<JsonElement>();
|
||||
if (body.ValueKind != JsonValueKind.Object)
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
||||
|
||||
double angularX = body.TryGetProperty("x", out var xProp) ? xProp.GetDouble() : 0.0;
|
||||
double angularY = body.TryGetProperty("y", out var yProp) ? yProp.GetDouble() : 0.0;
|
||||
double angularZ = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
|
||||
|
||||
// Wrap synchronous native calls in Task.Run to avoid blocking the request thread
|
||||
bool result = await Task.Run(() => service.SetTwistAngular(angularX, angularY, angularZ));
|
||||
return result
|
||||
? Results.Ok(new { status = "success", message = "Angular twist set", twist = new { x = angularX, y = angularY, z = angularZ } })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to set angular twist" });
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
|
||||
}
|
||||
});
|
||||
|
||||
// Navigation data - Current pose
|
||||
navApi.MapGet("/pose/current", ([FromServices] NavigationIntegrationService service) => {
|
||||
var pose = service.GetRobotPose();
|
||||
return pose.HasValue
|
||||
? Results.Ok(new {
|
||||
status = "success",
|
||||
pose = new {
|
||||
position = new { x = pose.Value.x, y = pose.Value.y, z = pose.Value.z },
|
||||
orientation = new { x = pose.Value.qx, y = pose.Value.qy, z = pose.Value.qz, w = pose.Value.qw },
|
||||
frameId = pose.Value.frameId
|
||||
}
|
||||
})
|
||||
: Results.NotFound(new { status = "error", message = "No pose available" });
|
||||
});
|
||||
|
||||
// Navigation data - Current pose 2D
|
||||
navApi.MapGet("/pose/current2d", ([FromServices] NavigationIntegrationService service) => {
|
||||
var pose = service.GetRobotPose2D();
|
||||
return pose.HasValue
|
||||
? Results.Ok(new {
|
||||
status = "success",
|
||||
pose = new { x = pose.Value.x, y = pose.Value.y, theta = pose.Value.theta }
|
||||
})
|
||||
: Results.NotFound(new { status = "error", message = "No pose available" });
|
||||
});
|
||||
|
||||
// Navigation data - Current twist
|
||||
navApi.MapGet("/twist", ([FromServices] NavigationIntegrationService service) => {
|
||||
var twist = service.GetTwist();
|
||||
return twist.HasValue
|
||||
? Results.Ok(new {
|
||||
status = "success",
|
||||
twist = new {
|
||||
linear = new { x = twist.Value.x, y = twist.Value.y },
|
||||
angular = new { z = twist.Value.theta },
|
||||
frameId = twist.Value.frameId
|
||||
}
|
||||
})
|
||||
: Results.NotFound(new { status = "error", message = "No twist available" });
|
||||
});
|
||||
|
||||
// Navigation data - Feedback
|
||||
navApi.MapGet("/feedback", ([FromServices] NavigationIntegrationService service) => {
|
||||
var feedback = service.GetFeedback();
|
||||
return feedback != null
|
||||
? Results.Ok(new {
|
||||
status = "success",
|
||||
feedback = new {
|
||||
navigationState = feedback.NavigationState,
|
||||
stateString = feedback.StateString,
|
||||
feedbackString = feedback.FeedbackString,
|
||||
currentPose = new {
|
||||
x = feedback.CurrentPose.X,
|
||||
y = feedback.CurrentPose.Y,
|
||||
theta = feedback.CurrentPose.Theta
|
||||
},
|
||||
goalChecked = feedback.GoalChecked,
|
||||
isReady = feedback.IsReady
|
||||
}
|
||||
})
|
||||
: Results.NotFound(new { status = "error", message = "No feedback available" });
|
||||
});
|
||||
|
||||
// Navigation data - Global path (navigation_get_global_data)
|
||||
navApi.MapGet("/global_data/path", async ([FromServices] NavigationIntegrationService service) => {
|
||||
var globalPath = await Task.Run(() => service.GetGlobalPathData());
|
||||
if (globalPath == null)
|
||||
{
|
||||
// Return OK with "no_data" status instead of NotFound to prevent log spam
|
||||
return Results.Ok(new { status = "no_data", message = "No global planner data available yet" });
|
||||
}
|
||||
|
||||
return Results.Ok(new
|
||||
{
|
||||
status = "success",
|
||||
source = "navigation_get_global_data",
|
||||
plan = new
|
||||
{
|
||||
frameId = globalPath.FrameId,
|
||||
pointCount = globalPath.Points.Count,
|
||||
points = globalPath.Points.Select(point => new
|
||||
{
|
||||
x = point.X,
|
||||
y = point.Y,
|
||||
theta = point.Theta
|
||||
})
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
// Navigation data - Local path (navigation_get_local_data)
|
||||
navApi.MapGet("/local_data/path", async (
|
||||
[FromServices] NavigationIntegrationService service,
|
||||
[FromServices] XlocIntegrationService xlocService,
|
||||
[FromServices] OdometryService odometryService) => {
|
||||
var localPath = await Task.Run(() => service.GetLocalPathData());
|
||||
if (localPath == null)
|
||||
{
|
||||
// Return OK with "no_data" status instead of NotFound to prevent log spam
|
||||
return Results.Ok(new { status = "no_data", message = "No local planner path data available yet" });
|
||||
}
|
||||
|
||||
// Transform local path to map frame so it is anchored to robot on the map view.
|
||||
// Local path is typically in "odom" (or base frame). Map view is in "map".
|
||||
var maybeMapPose = xlocService.GetCurrentPose2D(); // map->base pose
|
||||
var odom = odometryService.CurrentOdometry; // odom->base pose
|
||||
var oq = odom.Pose.Pose.Orientation;
|
||||
var odomYaw = Math.Atan2(
|
||||
2.0 * (oq.W * oq.Z + oq.X * oq.Y),
|
||||
1.0 - 2.0 * (oq.Y * oq.Y + oq.Z * oq.Z)
|
||||
);
|
||||
|
||||
double mapBaseX = maybeMapPose?.x ?? 0.0;
|
||||
double mapBaseY = maybeMapPose?.y ?? 0.0;
|
||||
double mapBaseYaw = maybeMapPose?.yaw ?? 0.0;
|
||||
|
||||
// odom->base
|
||||
double odomBaseX = odom.Pose.Pose.Position.X;
|
||||
double odomBaseY = odom.Pose.Pose.Position.Y;
|
||||
double odomBaseYaw = odomYaw;
|
||||
|
||||
// Compute map->odom = map->base ⊕ inverse(odom->base)
|
||||
// 2D composition:
|
||||
// inv(odom->base): (-R^T t, -yaw)
|
||||
// map->odom translation:
|
||||
// t_mo = t_mb + R(yaw_mb) * (-R(yaw_ob)^T * t_ob)
|
||||
// yaw_mo = yaw_mb - yaw_ob
|
||||
double yawMapOdom = mapBaseYaw - odomBaseYaw;
|
||||
double cosOb = Math.Cos(odomBaseYaw);
|
||||
double sinOb = Math.Sin(odomBaseYaw);
|
||||
// -R^T * t_ob
|
||||
double invX = -(cosOb * odomBaseX + sinOb * odomBaseY);
|
||||
double invY = -(-sinOb * odomBaseX + cosOb * odomBaseY);
|
||||
double cosMb = Math.Cos(mapBaseYaw);
|
||||
double sinMb = Math.Sin(mapBaseYaw);
|
||||
double mapOdomX = mapBaseX + (cosMb * invX - sinMb * invY);
|
||||
double mapOdomY = mapBaseY + (sinMb * invX + cosMb * invY);
|
||||
|
||||
// Helper: transform point from odom to map using map->odom
|
||||
static (double x, double y) TransformOdomToMap(double mapOdomX, double mapOdomY, double yawMapOdom, double xOdom, double yOdom)
|
||||
{
|
||||
double c = Math.Cos(yawMapOdom);
|
||||
double s = Math.Sin(yawMapOdom);
|
||||
return (mapOdomX + c * xOdom - s * yOdom, mapOdomY + s * xOdom + c * yOdom);
|
||||
}
|
||||
|
||||
// Helper: transform point from base to map using map->base
|
||||
static (double x, double y) TransformBaseToMap(double mapBaseX, double mapBaseY, double yawMapBase, double xBase, double yBase)
|
||||
{
|
||||
double c = Math.Cos(yawMapBase);
|
||||
double s = Math.Sin(yawMapBase);
|
||||
return (mapBaseX + c * xBase - s * yBase, mapBaseY + s * xBase + c * yBase);
|
||||
}
|
||||
|
||||
string originalFrameId = localPath.FrameId;
|
||||
var pointsOut = localPath.Points.Select(point =>
|
||||
{
|
||||
double x = point.X;
|
||||
double y = point.Y;
|
||||
double theta = point.Theta;
|
||||
|
||||
// If planner reports odom frame, convert to map frame.
|
||||
// If planner reports base frame, convert using map->base.
|
||||
if (string.Equals(originalFrameId, "odom", StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
var (xm, ym) = TransformOdomToMap(mapOdomX, mapOdomY, yawMapOdom, x, y);
|
||||
return new { x = xm, y = ym, theta = theta + yawMapOdom };
|
||||
}
|
||||
if (string.Equals(originalFrameId, "base_link", StringComparison.OrdinalIgnoreCase) ||
|
||||
string.Equals(originalFrameId, "base_footprint", StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
var (xm, ym) = TransformBaseToMap(mapBaseX, mapBaseY, mapBaseYaw, x, y);
|
||||
return new { x = xm, y = ym, theta = theta + mapBaseYaw };
|
||||
}
|
||||
|
||||
// Unknown frame: return as-is.
|
||||
return new { x, y, theta };
|
||||
}).ToList();
|
||||
|
||||
return Results.Ok(new
|
||||
{
|
||||
status = "success",
|
||||
source = "navigation_get_local_data",
|
||||
plan = new
|
||||
{
|
||||
frameId = "map",
|
||||
originalFrameId = originalFrameId,
|
||||
pointCount = pointsOut.Count,
|
||||
points = pointsOut
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
// Navigation data - Local cost map (navigation_get_local_data)
|
||||
navApi.MapGet("/local_data/costmap", async ([FromServices] NavigationIntegrationService service, [FromServices] OdometryService odometryService) => {
|
||||
var costMap = await Task.Run(() => service.GetCostMapData());
|
||||
if (costMap == null)
|
||||
{
|
||||
// Return OK with "no_data" status instead of NotFound to prevent log spam
|
||||
return Results.Ok(new { status = "no_data", message = "No local planner cost map data available yet" });
|
||||
}
|
||||
|
||||
var odom = odometryService.CurrentOdometry;
|
||||
var q = odom.Pose.Pose.Orientation;
|
||||
var odomYaw = Math.Atan2(
|
||||
2.0 * (q.W * q.Z + q.X * q.Y),
|
||||
1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z)
|
||||
);
|
||||
|
||||
return Results.Ok(new
|
||||
{
|
||||
status = "success",
|
||||
source = "navigation_get_local_data",
|
||||
hasFullMap = costMap.HasFullMap,
|
||||
isCostmapUpdated = costMap.IsCostmapUpdated,
|
||||
odometry = new
|
||||
{
|
||||
x = odom.Pose.Pose.Position.X,
|
||||
y = odom.Pose.Pose.Position.Y,
|
||||
yaw = odomYaw
|
||||
},
|
||||
costmap = new
|
||||
{
|
||||
frameId = costMap.FrameId,
|
||||
resolution = costMap.Resolution,
|
||||
width = costMap.Width,
|
||||
height = costMap.Height,
|
||||
origin = new
|
||||
{
|
||||
x = costMap.OriginX,
|
||||
y = costMap.OriginY,
|
||||
theta = costMap.OriginTheta
|
||||
},
|
||||
dataSize = costMap.HasFullMap ? costMap.Data.Length : 0,
|
||||
data = costMap.HasFullMap ? Convert.ToBase64String(costMap.Data) : null
|
||||
},
|
||||
costmapUpdate = new
|
||||
{
|
||||
frameId = costMap.UpdateFrameId,
|
||||
x = costMap.UpdateX,
|
||||
y = costMap.UpdateY,
|
||||
width = costMap.UpdateWidth,
|
||||
height = costMap.UpdateHeight,
|
||||
dataSize = costMap.IsCostmapUpdated ? costMap.UpdateData.Length : 0,
|
||||
data = costMap.IsCostmapUpdated ? Convert.ToBase64String(costMap.UpdateData) : null
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
// Navigation data - Robot footprint configuration
|
||||
navApi.MapGet("/robot_footprint", ([FromServices] NavigationIntegrationService service) => {
|
||||
var footprint = service.GetRobotFootprint();
|
||||
if (footprint == null || footprint.Length == 0)
|
||||
{
|
||||
return Results.NotFound(new { status = "error", message = "Robot footprint not configured" });
|
||||
}
|
||||
|
||||
return Results.Ok(new
|
||||
{
|
||||
status = "success",
|
||||
footprint = footprint.Select(point => new
|
||||
{
|
||||
x = point.X,
|
||||
y = point.Y,
|
||||
z = point.Z
|
||||
}).ToArray()
|
||||
});
|
||||
});
|
||||
|
||||
return app;
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,374 @@
|
||||
using System.Runtime.InteropServices;
|
||||
using RobotNet10.RobotApp.Xloc;
|
||||
using RobotNet10.Shared;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
|
||||
namespace RobotNet10.RobotApp.Navigation;
|
||||
|
||||
/// <summary>
|
||||
/// Extension methods to convert C# data structures to Navigation C API handles
|
||||
/// </summary>
|
||||
public static class NavigationConversionExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// Convert xloc_occupancy_grid_t to OccupancyGrid
|
||||
/// NOTE: Caller must free allocated memory using FreeNavigationOccupancyGrid
|
||||
/// </summary>
|
||||
public static OccupancyGrid ToNavigationOccupancyGrid(this xloc_occupancy_grid_t xlocGrid)
|
||||
{
|
||||
var navGrid = new OccupancyGrid
|
||||
{
|
||||
info = new MapMetaData
|
||||
{
|
||||
map_load_time = NavigationNativeInterface.time_create(),
|
||||
resolution = xlocGrid.resolution,
|
||||
width = xlocGrid.width,
|
||||
height = xlocGrid.height,
|
||||
origin = new Pose
|
||||
{
|
||||
position = new Point
|
||||
{
|
||||
x = xlocGrid.origin.position[0],
|
||||
y = xlocGrid.origin.position[1],
|
||||
z = xlocGrid.origin.position[2]
|
||||
},
|
||||
orientation = new Quaternion
|
||||
{
|
||||
x = xlocGrid.origin.orientation[0],
|
||||
y = xlocGrid.origin.orientation[1],
|
||||
z = xlocGrid.origin.orientation[2],
|
||||
w = xlocGrid.origin.orientation[3]
|
||||
}
|
||||
}
|
||||
},
|
||||
data = IntPtr.Zero
|
||||
};
|
||||
|
||||
// Marshal frame_id string
|
||||
if (xlocGrid.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
navGrid.header.frame_id = Marshal.StringToHGlobalAnsi(
|
||||
Marshal.PtrToStringAnsi(xlocGrid.header.frame_id) ?? string.Empty);
|
||||
}
|
||||
|
||||
// Copy occupancy data
|
||||
if (xlocGrid.data != IntPtr.Zero && xlocGrid.data_length > 0)
|
||||
{
|
||||
navGrid.data = Marshal.AllocHGlobal((int)xlocGrid.data_length);
|
||||
navGrid.data_count = new UIntPtr(xlocGrid.data_length);
|
||||
byte[] data = new byte[xlocGrid.data_length];
|
||||
Marshal.Copy(xlocGrid.data, data, 0, (int)xlocGrid.data_length);
|
||||
Marshal.Copy(data, 0, navGrid.data, (int)xlocGrid.data_length);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Ensure data_count is set to 0 when no data
|
||||
navGrid.data_count = UIntPtr.Zero;
|
||||
}
|
||||
|
||||
return navGrid;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert Shared.Sensor.LaserScan to NavigationInterop.LaserScan
|
||||
/// NOTE: Caller must free allocated memory using FreeNavigationLaserScan
|
||||
/// </summary>
|
||||
public static LaserScan ToNavigationLaserScan(this global::RobotNet10.Shared.Sensor.LaserScan scan)
|
||||
{
|
||||
// Create header manually with Marshal-allocated frame_id so we can safely free it after dispatch
|
||||
var timeSpan = scan.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch;
|
||||
Header scanHeader = new Header
|
||||
{
|
||||
seq = scan.Header.Seq,
|
||||
sec = (uint)timeSpan.TotalSeconds,
|
||||
nsec = (uint)((timeSpan.Ticks % TimeSpan.TicksPerSecond) * 100),
|
||||
frame_id = Marshal.StringToHGlobalAnsi(scan.Header.FrameId ?? string.Empty)
|
||||
};
|
||||
|
||||
var navScan = new LaserScan
|
||||
{
|
||||
header = scanHeader,
|
||||
angle_min = (float)scan.AngleMin,
|
||||
angle_max = (float)scan.AngleMax,
|
||||
angle_increment = (float)scan.AngleIncrement,
|
||||
time_increment = (float)scan.TimeIncrement,
|
||||
scan_time = (float)scan.ScanTime,
|
||||
range_min = (float)scan.RangeMin,
|
||||
range_max = (float)scan.RangeMax,
|
||||
ranges = IntPtr.Zero,
|
||||
ranges_count = 0,
|
||||
intensities = IntPtr.Zero,
|
||||
intensities_count = 0,
|
||||
};
|
||||
|
||||
// Allocate and copy ranges array
|
||||
if (scan.Ranges != null && scan.Ranges.Length > 0)
|
||||
{
|
||||
float[] rangesFloat = Array.ConvertAll(scan.Ranges, value => (float)value);
|
||||
int rangesSize = rangesFloat.Length * sizeof(float);
|
||||
navScan.ranges = Marshal.AllocHGlobal(rangesSize);
|
||||
Marshal.Copy(rangesFloat, 0, navScan.ranges, rangesFloat.Length);
|
||||
navScan.ranges_count = (nuint)rangesFloat.Length;
|
||||
}
|
||||
|
||||
// Allocate and copy intensities array
|
||||
if (scan.Intensities != null && scan.Intensities.Length > 0)
|
||||
{
|
||||
float[] intensitiesFloat = Array.ConvertAll(scan.Intensities, value => (float)value);
|
||||
int intensitiesSize = intensitiesFloat.Length * sizeof(float);
|
||||
navScan.intensities = Marshal.AllocHGlobal(intensitiesSize);
|
||||
Marshal.Copy(intensitiesFloat, 0, navScan.intensities, intensitiesFloat.Length);
|
||||
navScan.intensities_count = (nuint)intensitiesFloat.Length;
|
||||
}
|
||||
|
||||
return navScan;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert Odometry to Odometry
|
||||
/// NOTE: Caller must free allocated memory using FreeNavigationOdometry
|
||||
/// </summary>
|
||||
public static Odometry ToNavigationOdometry(this global::RobotNet10.Shared.Sensor.Odometry odom)
|
||||
{
|
||||
// Create header using header_create (like in Program.cs)
|
||||
Header odomHeader = NavigationNativeInterface.header_create(odom.Header.FrameId ?? string.Empty);
|
||||
|
||||
// Update header with sequence and timestamp
|
||||
odomHeader.seq = odom.Header.Seq;
|
||||
odomHeader.sec = (uint)(odom.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch).TotalSeconds;
|
||||
odomHeader.nsec = (uint)(((odom.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch).Ticks % TimeSpan.TicksPerSecond) * 100);
|
||||
|
||||
var navOdom = new Odometry
|
||||
{
|
||||
header = odomHeader,
|
||||
child_frame_id = IntPtr.Zero,
|
||||
pose = new PoseWithCovariance
|
||||
{
|
||||
pose = new Pose
|
||||
{
|
||||
position = new Point
|
||||
{
|
||||
x = odom.Pose.Pose.Position.X,
|
||||
y = odom.Pose.Pose.Position.Y,
|
||||
z = odom.Pose.Pose.Position.Z
|
||||
},
|
||||
orientation = new Quaternion
|
||||
{
|
||||
x = odom.Pose.Pose.Orientation.X,
|
||||
y = odom.Pose.Pose.Orientation.Y,
|
||||
z = odom.Pose.Pose.Orientation.Z,
|
||||
w = odom.Pose.Pose.Orientation.W
|
||||
}
|
||||
},
|
||||
covariance = IntPtr.Zero,
|
||||
covariance_count = UIntPtr.Zero
|
||||
},
|
||||
twist = new TwistWithCovariance
|
||||
{
|
||||
twist = new Twist
|
||||
{
|
||||
linear = new Vector3
|
||||
{
|
||||
x = odom.Twist.Twist.Linear.X,
|
||||
y = odom.Twist.Twist.Linear.Y,
|
||||
z = odom.Twist.Twist.Linear.Z
|
||||
},
|
||||
angular = new Vector3
|
||||
{
|
||||
x = odom.Twist.Twist.Angular.X,
|
||||
y = odom.Twist.Twist.Angular.Y,
|
||||
z = odom.Twist.Twist.Angular.Z
|
||||
}
|
||||
},
|
||||
covariance = IntPtr.Zero,
|
||||
covariance_count = UIntPtr.Zero
|
||||
}
|
||||
};
|
||||
|
||||
// Marshal child_frame_id string
|
||||
if (!string.IsNullOrEmpty(odom.ChildFrameId))
|
||||
{
|
||||
navOdom.child_frame_id = Marshal.StringToHGlobalAnsi(odom.ChildFrameId);
|
||||
}
|
||||
|
||||
// Allocate and copy pose covariance
|
||||
if (odom.Pose.Covariance != null && odom.Pose.Covariance.Length == 36)
|
||||
{
|
||||
int covarianceSize = 36 * sizeof(double);
|
||||
navOdom.pose.covariance = Marshal.AllocHGlobal(covarianceSize);
|
||||
Marshal.Copy(odom.Pose.Covariance, 0, navOdom.pose.covariance, 36);
|
||||
navOdom.pose.covariance_count = new UIntPtr(36);
|
||||
}
|
||||
|
||||
// Allocate and copy twist covariance
|
||||
if (odom.Twist.Covariance != null && odom.Twist.Covariance.Length == 36)
|
||||
{
|
||||
int covarianceSize = 36 * sizeof(double);
|
||||
navOdom.twist.covariance = Marshal.AllocHGlobal(covarianceSize);
|
||||
Marshal.Copy(odom.Twist.Covariance, 0, navOdom.twist.covariance, 36);
|
||||
navOdom.twist.covariance_count = new UIntPtr(36);
|
||||
}
|
||||
|
||||
return navOdom;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Allocate unmanaged memory for OccupancyGrid and return IntPtr
|
||||
/// NOTE: Caller must free using FreeNavigationOccupancyGridPtr
|
||||
/// </summary>
|
||||
public static IntPtr AllocateNavigationOccupancyGrid(OccupancyGrid grid)
|
||||
{
|
||||
int size = Marshal.SizeOf<OccupancyGrid>();
|
||||
IntPtr ptr = Marshal.AllocHGlobal(size);
|
||||
Marshal.StructureToPtr(grid, ptr, false);
|
||||
return ptr;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Allocate unmanaged memory for LaserScanHandle and return IntPtr
|
||||
/// NOTE: Caller must free using FreeNavigationLaserScanPtr
|
||||
/// </summary>
|
||||
public static IntPtr AllocateNavigationLaserScan(LaserScan scan)
|
||||
{
|
||||
int size = Marshal.SizeOf<LaserScan>();
|
||||
IntPtr ptr = Marshal.AllocHGlobal(size);
|
||||
Marshal.StructureToPtr(scan, ptr, false);
|
||||
return ptr;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Allocate unmanaged memory for Odometry and return IntPtr
|
||||
/// NOTE: Caller must free using FreeNavigationOdometryPtr
|
||||
/// </summary>
|
||||
public static IntPtr AllocateNavigationOdometry(Odometry odom)
|
||||
{
|
||||
int size = Marshal.SizeOf<Odometry>();
|
||||
IntPtr ptr = Marshal.AllocHGlobal(size);
|
||||
Marshal.StructureToPtr(odom, ptr, false);
|
||||
return ptr;
|
||||
}
|
||||
|
||||
#region Memory Management
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for OccupancyGrid (both struct and IntPtr)
|
||||
/// </summary>
|
||||
public static void FreeNavigationOccupancyGridPtr(IntPtr ptr)
|
||||
{
|
||||
if (ptr == IntPtr.Zero)
|
||||
return;
|
||||
|
||||
var grid = Marshal.PtrToStructure<OccupancyGrid>(ptr);
|
||||
FreeNavigationOccupancyGrid(ref grid);
|
||||
Marshal.FreeHGlobal(ptr);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for OccupancyGrid
|
||||
/// </summary>
|
||||
public static void FreeNavigationOccupancyGrid(ref OccupancyGrid grid)
|
||||
{
|
||||
if (grid.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(grid.header.frame_id);
|
||||
grid.header.frame_id = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (grid.data != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(grid.data);
|
||||
grid.data = IntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for LaserScanHandle (both struct and IntPtr)
|
||||
/// </summary>
|
||||
public static void FreeNavigationLaserScanPtr(IntPtr ptr)
|
||||
{
|
||||
if (ptr == IntPtr.Zero)
|
||||
return;
|
||||
|
||||
var scan = Marshal.PtrToStructure<LaserScan>(ptr);
|
||||
FreeNavigationLaserScan(ref scan);
|
||||
Marshal.FreeHGlobal(ptr);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for LaserScanHandle
|
||||
/// </summary>
|
||||
public static void FreeNavigationLaserScan(ref LaserScan scan)
|
||||
{
|
||||
// frame_id is allocated by Marshal.StringToHGlobalAnsi(), so use Marshal.FreeHGlobal()
|
||||
if (scan.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(scan.header.frame_id);
|
||||
scan.header.frame_id = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// ranges and intensities are allocated by Marshal.AllocHGlobal(), so use Marshal.FreeHGlobal()
|
||||
if (scan.ranges != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(scan.ranges);
|
||||
scan.ranges = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (scan.intensities != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(scan.intensities);
|
||||
scan.intensities = IntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for Odometry (both struct and IntPtr)
|
||||
/// </summary>
|
||||
public static void FreeNavigationOdometryPtr(IntPtr ptr)
|
||||
{
|
||||
if (ptr == IntPtr.Zero)
|
||||
return;
|
||||
|
||||
var odom = Marshal.PtrToStructure<Odometry>(ptr);
|
||||
FreeNavigationOdometry(ref odom);
|
||||
Marshal.FreeHGlobal(ptr);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for Odometry
|
||||
/// </summary>
|
||||
public static void FreeNavigationOdometry(ref Odometry odom)
|
||||
{
|
||||
// header.frame_id is allocated by header_create() using strdup() (malloc), so use nav_c_api_free_string()
|
||||
if (odom.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
NavigationNativeInterface.nav_c_api_free_string(odom.header.frame_id);
|
||||
odom.header.frame_id = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// child_frame_id is allocated by Marshal.StringToHGlobalAnsi(), so use Marshal.FreeHGlobal()
|
||||
if (odom.child_frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(odom.child_frame_id);
|
||||
odom.child_frame_id = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// covariance arrays are allocated by Marshal.AllocHGlobal(), so use Marshal.FreeHGlobal()
|
||||
if (odom.pose.covariance != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(odom.pose.covariance);
|
||||
odom.pose.covariance = IntPtr.Zero;
|
||||
odom.pose.covariance_count = UIntPtr.Zero;
|
||||
}
|
||||
|
||||
if (odom.twist.covariance != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(odom.twist.covariance);
|
||||
odom.twist.covariance = IntPtr.Zero;
|
||||
odom.twist.covariance_count = UIntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,951 @@
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace RobotNet10.RobotApp.Navigation;
|
||||
|
||||
/// <summary>
|
||||
/// C-compatible structs for Navigation C API interop
|
||||
/// Based on /home/robotics/sonvh/pnkx_nav_core/src/APIs/c_api/include/nav_c_api.h
|
||||
/// </summary>
|
||||
|
||||
/// <summary>
|
||||
/// Navigation states, including planning and controller status
|
||||
/// </summary>
|
||||
public enum NavigationState : int
|
||||
{
|
||||
Pending = 0,
|
||||
Active = 1,
|
||||
Preempted = 2,
|
||||
Succeeded = 3,
|
||||
Aborted = 4,
|
||||
Rejected = 5,
|
||||
Preempting = 6,
|
||||
Recalling = 7,
|
||||
Recalled = 8,
|
||||
Lost = 9,
|
||||
Planning = 10,
|
||||
Controlling = 11,
|
||||
Clearing = 12,
|
||||
Paused = 13
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Point structure (x, y, z)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Point
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Pose2D structure (x, y, theta)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Pose2D
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double theta;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Pose2DStamped
|
||||
{
|
||||
public Header header;
|
||||
public Pose2D pose;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Path2D
|
||||
{
|
||||
public Header header;
|
||||
public IntPtr poses;
|
||||
public nuint poses_count;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Twist2D structure (x, y, theta velocities)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Twist2D
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double theta;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Quaternion structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Quaternion
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
public double w;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Position structure (alias for Point to match C API)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Position
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
}
|
||||
|
||||
// Point is the same as Position in C API
|
||||
// public using Point = Position;
|
||||
|
||||
/// <summary>
|
||||
/// Pose structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Pose
|
||||
{
|
||||
public Point position; // Use Point to match C API exactly
|
||||
public Quaternion orientation;
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Header structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Header
|
||||
{
|
||||
public uint seq;
|
||||
public uint sec; // uint32_t
|
||||
public uint nsec;
|
||||
public IntPtr frame_id; // char* - must be allocated and freed
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PoseStamped structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PoseStamped
|
||||
{
|
||||
public Header header;
|
||||
public Pose pose;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Twist2DStamped structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Twist2DStamped
|
||||
{
|
||||
public Header header;
|
||||
public Twist2D velocity;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Vector3 structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Vector3
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// OccupancyGrid structure
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct OccupancyGrid
|
||||
{
|
||||
public Header header;
|
||||
public MapMetaData info;
|
||||
public IntPtr data;
|
||||
public nuint data_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct OccupancyGridUpdate
|
||||
{
|
||||
public Header header;
|
||||
public int x;
|
||||
public int y;
|
||||
public uint width;
|
||||
public uint height;
|
||||
public IntPtr data;
|
||||
public nuint data_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Point32
|
||||
{
|
||||
public float x;
|
||||
public float y;
|
||||
public float z;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Polygon
|
||||
{
|
||||
public IntPtr points;
|
||||
public nuint points_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PolygonStamped
|
||||
{
|
||||
public Header header;
|
||||
public Polygon polygon;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PlannerDataOutput
|
||||
{
|
||||
public Path2D plan;
|
||||
public OccupancyGrid costmap;
|
||||
public OccupancyGridUpdate costmap_update;
|
||||
[MarshalAs(UnmanagedType.I1)]
|
||||
public bool is_costmap_updated;
|
||||
public PolygonStamped footprint;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Time structure (sec, nsec)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Time
|
||||
{
|
||||
public uint sec;
|
||||
public uint nsec;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct MapMetaData
|
||||
{
|
||||
public Time map_load_time; // Time when map was loaded
|
||||
public float resolution; // meters per cell
|
||||
public uint width; // cells
|
||||
public uint height; // cells
|
||||
public Pose origin; // pose of cell (0,0) in map frame
|
||||
}
|
||||
/// <summary>
|
||||
/// Navigation feedback structure
|
||||
/// </summary>
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NavFeedback
|
||||
{
|
||||
public NavigationState navigation_state;
|
||||
public IntPtr feed_back_str; // char*; free with nav_c_api_free_string
|
||||
public Pose2D current_pose;
|
||||
[MarshalAs(UnmanagedType.I1)]
|
||||
public bool goal_checked;
|
||||
[MarshalAs(UnmanagedType.I1)]
|
||||
public bool is_ready;
|
||||
}
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Odometry
|
||||
{
|
||||
public Header header;
|
||||
public IntPtr child_frame_id; // char* - must be allocated and freed
|
||||
public PoseWithCovariance pose;
|
||||
public TwistWithCovariance twist;
|
||||
}
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct PoseWithCovariance{
|
||||
public Pose pose;
|
||||
public IntPtr covariance; // double* - array of 36 doubles (6x6 matrix)
|
||||
public nuint covariance_count; // size_t - should be 36
|
||||
}
|
||||
// [StructLayout(LayoutKind.Sequential)]
|
||||
// public struct Position{
|
||||
// public double x;
|
||||
// public double y;
|
||||
// public double z;
|
||||
// }
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct TwistWithCovariance{
|
||||
public Twist twist;
|
||||
public IntPtr covariance; // double* - array of 36 doubles (6x6 matrix)
|
||||
public nuint covariance_count; // size_t - should be 36
|
||||
}
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Twist{
|
||||
public Vector3 linear;
|
||||
public Vector3 angular;
|
||||
}
|
||||
// [StructLayout(LayoutKind.Sequential)]
|
||||
// public struct Vector3{
|
||||
// public double x;
|
||||
// public double y;
|
||||
// public double z;
|
||||
// }
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct LaserScan
|
||||
{
|
||||
public Header header;
|
||||
public float angle_min;
|
||||
public float angle_max;
|
||||
public float angle_increment;
|
||||
public float time_increment;
|
||||
public float scan_time;
|
||||
public float range_min;
|
||||
public float range_max;
|
||||
public IntPtr ranges;
|
||||
public nuint ranges_count;
|
||||
public IntPtr intensities;
|
||||
public nuint intensities_count;
|
||||
}
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct ControlPoint
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double weight;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct ActionParameter
|
||||
{
|
||||
public IntPtr key; // char*
|
||||
public IntPtr value; // char*
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct NodePosition
|
||||
{
|
||||
public double x;
|
||||
public double y;
|
||||
public double theta;
|
||||
public float allowedDeviationXY;
|
||||
public float allowedDeviationTheta;
|
||||
public IntPtr mapId; // char*
|
||||
public IntPtr mapDescription; // char*
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Trajectory
|
||||
{
|
||||
public uint degree;
|
||||
public IntPtr knotVector; // double*
|
||||
public nuint knotVector_count;
|
||||
public IntPtr controlPoints; // ControlPoint*
|
||||
public nuint controlPoints_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Action
|
||||
{
|
||||
public IntPtr actionType; // char*
|
||||
public IntPtr actionId; // char*
|
||||
public IntPtr actionDescription;// char*
|
||||
public IntPtr blockingType; // char*
|
||||
public IntPtr actionParameters; // ActionParameter*
|
||||
public nuint actionParameters_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Node
|
||||
{
|
||||
public IntPtr nodeId; // char*
|
||||
public int sequenceId;
|
||||
public IntPtr nodeDescription; // char*
|
||||
public byte released;
|
||||
public NodePosition nodePosition;
|
||||
public IntPtr actions; // Action*
|
||||
public nuint actions_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Edge
|
||||
{
|
||||
public IntPtr edgeId; // char*
|
||||
public int sequenceId;
|
||||
public IntPtr edgeDescription; // char*
|
||||
public byte released;
|
||||
public IntPtr startNodeId; // char*
|
||||
public IntPtr endNodeId; // char*
|
||||
public double maxSpeed;
|
||||
public double maxHeight;
|
||||
public double minHeight;
|
||||
public double orientation;
|
||||
public IntPtr orientationType; // char*
|
||||
public IntPtr direction; // char*
|
||||
public byte rotationAllowed;
|
||||
public double maxRotationSpeed;
|
||||
public Trajectory trajectory;
|
||||
public double length;
|
||||
public IntPtr actions; // Action*
|
||||
public nuint actions_count;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct Order
|
||||
{
|
||||
public int headerId;
|
||||
public IntPtr timestamp; // char*
|
||||
public IntPtr version; // char*
|
||||
public IntPtr manufacturer; // char*
|
||||
public IntPtr serialNumber; // char*
|
||||
public IntPtr orderId; // char*
|
||||
public uint orderUpdateId;
|
||||
public IntPtr nodes; // Node*
|
||||
public nuint nodes_count;
|
||||
public IntPtr edges; // Edge*
|
||||
public nuint edges_count;
|
||||
public IntPtr zoneSetId; // char*
|
||||
}
|
||||
/// <summary>
|
||||
/// OrderHandle - wrapper for OrderHandle (void* in C)
|
||||
/// Represents a pointer to a robot_protocol_msgs::Order object
|
||||
/// </summary>
|
||||
public class OrderHandle : IDisposable
|
||||
{
|
||||
private IntPtr _handle;
|
||||
private bool _disposed = false;
|
||||
private readonly bool _ownsHandle;
|
||||
|
||||
/// <summary>
|
||||
/// Create OrderHandle from IntPtr
|
||||
/// </summary>
|
||||
/// <param name="handle">Pointer to Order object</param>
|
||||
/// <param name="ownsHandle">Whether this handle owns the memory (will free on dispose)</param>
|
||||
public OrderHandle(IntPtr handle, bool ownsHandle = false)
|
||||
{
|
||||
_handle = handle;
|
||||
_ownsHandle = ownsHandle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get the underlying IntPtr
|
||||
/// </summary>
|
||||
public IntPtr Handle => _handle;
|
||||
|
||||
/// <summary>
|
||||
/// Check if handle is valid (not zero)
|
||||
/// </summary>
|
||||
public bool IsValid => _handle != IntPtr.Zero;
|
||||
|
||||
/// <summary>
|
||||
/// Implicit conversion to IntPtr
|
||||
/// </summary>
|
||||
public static implicit operator IntPtr(OrderHandle orderHandle) => orderHandle?._handle ?? IntPtr.Zero;
|
||||
|
||||
/// <summary>
|
||||
/// Explicit conversion from IntPtr
|
||||
/// </summary>
|
||||
public static explicit operator OrderHandle(IntPtr handle) => new OrderHandle(handle);
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
Dispose(true);
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
protected virtual void Dispose(bool disposing)
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
|
||||
if (disposing && _ownsHandle && _handle != IntPtr.Zero)
|
||||
{
|
||||
// Free the Order object if we own it
|
||||
// Note: This requires navigation_free_order function
|
||||
// NavigationNativeInterface.navigation_free_order(_handle);
|
||||
_handle = IntPtr.Zero;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
}
|
||||
|
||||
~OrderHandle()
|
||||
{
|
||||
Dispose(false);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order data structure (C# representation of robot_protocol_msgs::Order)
|
||||
/// This is a simplified version for API usage
|
||||
/// </summary>
|
||||
public class OrderData
|
||||
{
|
||||
public int HeaderId { get; set; }
|
||||
public string Timestamp { get; set; } = string.Empty;
|
||||
public string Version { get; set; } = string.Empty;
|
||||
public string Manufacturer { get; set; } = string.Empty;
|
||||
public string SerialNumber { get; set; } = string.Empty;
|
||||
public string OrderId { get; set; } = string.Empty;
|
||||
public uint OrderUpdateId { get; set; }
|
||||
public string? ZoneSetId { get; set; }
|
||||
public List<OrderNodeData> Nodes { get; set; } = new();
|
||||
public List<OrderEdgeData> Edges { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Node data
|
||||
/// </summary>
|
||||
public class OrderNodeData
|
||||
{
|
||||
public string NodeId { get; set; } = string.Empty;
|
||||
public int SequenceId { get; set; }
|
||||
public string NodeDescription { get; set; } = string.Empty;
|
||||
public bool Released { get; set; }
|
||||
public OrderNodePositionData? NodePosition { get; set; }
|
||||
public List<OrderActionData> Actions { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Node Position data
|
||||
/// </summary>
|
||||
public class OrderNodePositionData
|
||||
{
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public double? Theta { get; set; }
|
||||
public double? AllowedDeviationXY { get; set; }
|
||||
public double? AllowedDeviationTheta { get; set; }
|
||||
public string? MapId { get; set; }
|
||||
public string? MapDescription { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Edge data
|
||||
/// </summary>
|
||||
public class OrderEdgeData
|
||||
{
|
||||
public string EdgeId { get; set; } = string.Empty;
|
||||
public int SequenceId { get; set; }
|
||||
public string? EdgeDescription { get; set; }
|
||||
public bool Released { get; set; }
|
||||
public string StartNodeId { get; set; } = string.Empty;
|
||||
public string EndNodeId { get; set; } = string.Empty;
|
||||
public double? MaxSpeed { get; set; }
|
||||
public double? MaxHeight { get; set; }
|
||||
public double? MinHeight { get; set; }
|
||||
public double Orientation { get; set; }
|
||||
/// <summary>VDA5050 can send number (e.g. 1) or string; we accept both via JsonElement.</summary>
|
||||
public JsonElement? OrientationType { get; set; }
|
||||
public string Direction { get; set; } = "forward";
|
||||
public bool? RotationAllowed { get; set; }
|
||||
public double? MaxRotationSpeed { get; set; }
|
||||
public double Length { get; set; }
|
||||
public OrderTrajectoryData? Trajectory { get; set; }
|
||||
public List<OrderActionData> Actions { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Trajectory data
|
||||
/// </summary>
|
||||
public class OrderTrajectoryData
|
||||
{
|
||||
public uint Degree { get; set; }
|
||||
public List<double> KnotVector { get; set; } = new();
|
||||
public List<OrderControlPointData> ControlPoints { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Control Point data
|
||||
/// </summary>
|
||||
public class OrderControlPointData
|
||||
{
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public double Weight { get; set; } = 1.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Action data
|
||||
/// </summary>
|
||||
public class OrderActionData
|
||||
{
|
||||
public string ActionType { get; set; } = string.Empty;
|
||||
public string ActionId { get; set; } = string.Empty;
|
||||
public string ActionDescription { get; set; } = string.Empty;
|
||||
public string BlockingType { get; set; } = "NONE";
|
||||
public List<OrderActionParameterData> ActionParameters { get; set; } = new();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Order Action Parameter data
|
||||
/// </summary>
|
||||
public class OrderActionParameterData
|
||||
{
|
||||
public string Key { get; set; } = string.Empty;
|
||||
public string Value { get; set; } = string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Helper class to convert OrderData to native Order struct
|
||||
/// </summary>
|
||||
public static class OrderConverter
|
||||
{
|
||||
/// <summary>
|
||||
/// Convert OrderData (managed) to Order (native struct)
|
||||
/// </summary>
|
||||
/// <param name="orderData">Managed order data</param>
|
||||
/// <returns>Native Order struct with allocated memory</returns>
|
||||
public static Order ConvertToNativeOrder(OrderData orderData)
|
||||
{
|
||||
var order = new Order
|
||||
{
|
||||
headerId = orderData.HeaderId,
|
||||
timestamp = Marshal.StringToHGlobalAnsi(orderData.Timestamp),
|
||||
version = Marshal.StringToHGlobalAnsi(orderData.Version),
|
||||
manufacturer = Marshal.StringToHGlobalAnsi(orderData.Manufacturer),
|
||||
serialNumber = Marshal.StringToHGlobalAnsi(orderData.SerialNumber),
|
||||
orderId = Marshal.StringToHGlobalAnsi(orderData.OrderId),
|
||||
orderUpdateId = orderData.OrderUpdateId,
|
||||
zoneSetId = Marshal.StringToHGlobalAnsi(orderData.ZoneSetId ?? string.Empty),
|
||||
nodes_count = (nuint)orderData.Nodes.Count,
|
||||
edges_count = (nuint)orderData.Edges.Count
|
||||
};
|
||||
|
||||
// Allocate and convert nodes
|
||||
if (orderData.Nodes.Count > 0)
|
||||
{
|
||||
int nodeSize = Marshal.SizeOf<Node>();
|
||||
order.nodes = Marshal.AllocHGlobal(nodeSize * orderData.Nodes.Count);
|
||||
|
||||
for (int i = 0; i < orderData.Nodes.Count; i++)
|
||||
{
|
||||
var nodeData = orderData.Nodes[i];
|
||||
var node = ConvertToNativeNode(nodeData);
|
||||
IntPtr nodePtr = IntPtr.Add(order.nodes, i * nodeSize);
|
||||
Marshal.StructureToPtr(node, nodePtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
order.nodes = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// Allocate and convert edges
|
||||
if (orderData.Edges.Count > 0)
|
||||
{
|
||||
int edgeSize = Marshal.SizeOf<Edge>();
|
||||
order.edges = Marshal.AllocHGlobal(edgeSize * orderData.Edges.Count);
|
||||
|
||||
for (int i = 0; i < orderData.Edges.Count; i++)
|
||||
{
|
||||
var edgeData = orderData.Edges[i];
|
||||
var edge = ConvertToNativeEdge(edgeData);
|
||||
IntPtr edgePtr = IntPtr.Add(order.edges, i * edgeSize);
|
||||
Marshal.StructureToPtr(edge, edgePtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
order.edges = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return order;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for native Order struct
|
||||
/// </summary>
|
||||
public static void FreeNativeOrder(ref Order order)
|
||||
{
|
||||
// Free string fields
|
||||
if (order.timestamp != IntPtr.Zero) Marshal.FreeHGlobal(order.timestamp);
|
||||
if (order.version != IntPtr.Zero) Marshal.FreeHGlobal(order.version);
|
||||
if (order.manufacturer != IntPtr.Zero) Marshal.FreeHGlobal(order.manufacturer);
|
||||
if (order.serialNumber != IntPtr.Zero) Marshal.FreeHGlobal(order.serialNumber);
|
||||
if (order.orderId != IntPtr.Zero) Marshal.FreeHGlobal(order.orderId);
|
||||
if (order.zoneSetId != IntPtr.Zero) Marshal.FreeHGlobal(order.zoneSetId);
|
||||
|
||||
// Free nodes
|
||||
if (order.nodes != IntPtr.Zero)
|
||||
{
|
||||
int nodeSize = Marshal.SizeOf<Node>();
|
||||
for (int i = 0; i < (int)order.nodes_count; i++)
|
||||
{
|
||||
IntPtr nodePtr = IntPtr.Add(order.nodes, i * nodeSize);
|
||||
Node node = Marshal.PtrToStructure<Node>(nodePtr);
|
||||
FreeNativeNode(ref node);
|
||||
}
|
||||
Marshal.FreeHGlobal(order.nodes);
|
||||
}
|
||||
|
||||
// Free edges
|
||||
if (order.edges != IntPtr.Zero)
|
||||
{
|
||||
int edgeSize = Marshal.SizeOf<Edge>();
|
||||
for (int i = 0; i < (int)order.edges_count; i++)
|
||||
{
|
||||
IntPtr edgePtr = IntPtr.Add(order.edges, i * edgeSize);
|
||||
Edge edge = Marshal.PtrToStructure<Edge>(edgePtr);
|
||||
FreeNativeEdge(ref edge);
|
||||
}
|
||||
Marshal.FreeHGlobal(order.edges);
|
||||
}
|
||||
}
|
||||
|
||||
private static Node ConvertToNativeNode(OrderNodeData nodeData)
|
||||
{
|
||||
var node = new Node
|
||||
{
|
||||
nodeId = Marshal.StringToHGlobalAnsi(nodeData.NodeId),
|
||||
sequenceId = nodeData.SequenceId,
|
||||
nodeDescription = Marshal.StringToHGlobalAnsi(nodeData.NodeDescription),
|
||||
released = (byte)(nodeData.Released ? 1 : 0),
|
||||
actions_count = (nuint)nodeData.Actions.Count
|
||||
};
|
||||
|
||||
// Convert node position
|
||||
if (nodeData.NodePosition != null)
|
||||
{
|
||||
node.nodePosition = new NodePosition
|
||||
{
|
||||
x = nodeData.NodePosition.X,
|
||||
y = nodeData.NodePosition.Y,
|
||||
theta = nodeData.NodePosition.Theta ?? 0.0,
|
||||
allowedDeviationXY = (float)(nodeData.NodePosition.AllowedDeviationXY ?? 0.0),
|
||||
allowedDeviationTheta = (float)(nodeData.NodePosition.AllowedDeviationTheta ?? 0.0),
|
||||
mapId = Marshal.StringToHGlobalAnsi(nodeData.NodePosition.MapId ?? string.Empty),
|
||||
mapDescription = Marshal.StringToHGlobalAnsi(nodeData.NodePosition.MapDescription ?? string.Empty)
|
||||
};
|
||||
}
|
||||
|
||||
// Convert actions
|
||||
if (nodeData.Actions.Count > 0)
|
||||
{
|
||||
int actionSize = Marshal.SizeOf<Action>();
|
||||
node.actions = Marshal.AllocHGlobal(actionSize * nodeData.Actions.Count);
|
||||
|
||||
for (int i = 0; i < nodeData.Actions.Count; i++)
|
||||
{
|
||||
var actionData = nodeData.Actions[i];
|
||||
var action = ConvertToNativeAction(actionData);
|
||||
IntPtr actionPtr = IntPtr.Add(node.actions, i * actionSize);
|
||||
Marshal.StructureToPtr(action, actionPtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
node.actions = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return node;
|
||||
}
|
||||
|
||||
private static void FreeNativeNode(ref Node node)
|
||||
{
|
||||
if (node.nodeId != IntPtr.Zero) Marshal.FreeHGlobal(node.nodeId);
|
||||
if (node.nodeDescription != IntPtr.Zero) Marshal.FreeHGlobal(node.nodeDescription);
|
||||
if (node.nodePosition.mapId != IntPtr.Zero) Marshal.FreeHGlobal(node.nodePosition.mapId);
|
||||
if (node.nodePosition.mapDescription != IntPtr.Zero) Marshal.FreeHGlobal(node.nodePosition.mapDescription);
|
||||
|
||||
if (node.actions != IntPtr.Zero)
|
||||
{
|
||||
int actionSize = Marshal.SizeOf<Action>();
|
||||
for (int i = 0; i < (int)node.actions_count; i++)
|
||||
{
|
||||
IntPtr actionPtr = IntPtr.Add(node.actions, i * actionSize);
|
||||
Action action = Marshal.PtrToStructure<Action>(actionPtr);
|
||||
FreeNativeAction(ref action);
|
||||
}
|
||||
Marshal.FreeHGlobal(node.actions);
|
||||
}
|
||||
}
|
||||
|
||||
private static string OrientationTypeToString(JsonElement? je)
|
||||
{
|
||||
if (!je.HasValue) return string.Empty;
|
||||
var e = je.Value;
|
||||
if (e.ValueKind == JsonValueKind.Number) return e.GetDouble().ToString(System.Globalization.CultureInfo.InvariantCulture);
|
||||
if (e.ValueKind == JsonValueKind.String) return e.GetString() ?? string.Empty;
|
||||
return string.Empty;
|
||||
}
|
||||
|
||||
private static Edge ConvertToNativeEdge(OrderEdgeData edgeData)
|
||||
{
|
||||
var edge = new Edge
|
||||
{
|
||||
edgeId = Marshal.StringToHGlobalAnsi(edgeData.EdgeId),
|
||||
sequenceId = edgeData.SequenceId,
|
||||
edgeDescription = Marshal.StringToHGlobalAnsi(edgeData.EdgeDescription ?? string.Empty),
|
||||
released = (byte)(edgeData.Released ? 1 : 0),
|
||||
startNodeId = Marshal.StringToHGlobalAnsi(edgeData.StartNodeId),
|
||||
endNodeId = Marshal.StringToHGlobalAnsi(edgeData.EndNodeId),
|
||||
maxSpeed = edgeData.MaxSpeed ?? 0.0,
|
||||
maxHeight = edgeData.MaxHeight ?? 0.0,
|
||||
minHeight = edgeData.MinHeight ?? 0.0,
|
||||
orientation = edgeData.Orientation,
|
||||
orientationType = Marshal.StringToHGlobalAnsi(OrientationTypeToString(edgeData.OrientationType)),
|
||||
direction = Marshal.StringToHGlobalAnsi(edgeData.Direction ?? string.Empty),
|
||||
rotationAllowed = (byte)((edgeData.RotationAllowed ?? false) ? 1 : 0),
|
||||
maxRotationSpeed = edgeData.MaxRotationSpeed ?? 0.0,
|
||||
length = edgeData.Length,
|
||||
actions_count = (nuint)edgeData.Actions.Count
|
||||
};
|
||||
|
||||
// Convert trajectory
|
||||
if (edgeData.Trajectory != null)
|
||||
{
|
||||
edge.trajectory = ConvertToNativeTrajectory(edgeData.Trajectory);
|
||||
}
|
||||
|
||||
// Convert actions
|
||||
if (edgeData.Actions.Count > 0)
|
||||
{
|
||||
int actionSize = Marshal.SizeOf<Action>();
|
||||
edge.actions = Marshal.AllocHGlobal(actionSize * edgeData.Actions.Count);
|
||||
|
||||
for (int i = 0; i < edgeData.Actions.Count; i++)
|
||||
{
|
||||
var actionData = edgeData.Actions[i];
|
||||
var action = ConvertToNativeAction(actionData);
|
||||
IntPtr actionPtr = IntPtr.Add(edge.actions, i * actionSize);
|
||||
Marshal.StructureToPtr(action, actionPtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
edge.actions = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return edge;
|
||||
}
|
||||
|
||||
private static void FreeNativeEdge(ref Edge edge)
|
||||
{
|
||||
if (edge.edgeId != IntPtr.Zero) Marshal.FreeHGlobal(edge.edgeId);
|
||||
if (edge.edgeDescription != IntPtr.Zero) Marshal.FreeHGlobal(edge.edgeDescription);
|
||||
if (edge.startNodeId != IntPtr.Zero) Marshal.FreeHGlobal(edge.startNodeId);
|
||||
if (edge.endNodeId != IntPtr.Zero) Marshal.FreeHGlobal(edge.endNodeId);
|
||||
if (edge.orientationType != IntPtr.Zero) Marshal.FreeHGlobal(edge.orientationType);
|
||||
if (edge.direction != IntPtr.Zero) Marshal.FreeHGlobal(edge.direction);
|
||||
|
||||
FreeNativeTrajectory(ref edge.trajectory);
|
||||
|
||||
if (edge.actions != IntPtr.Zero)
|
||||
{
|
||||
int actionSize = Marshal.SizeOf<Action>();
|
||||
for (int i = 0; i < (int)edge.actions_count; i++)
|
||||
{
|
||||
IntPtr actionPtr = IntPtr.Add(edge.actions, i * actionSize);
|
||||
Action action = Marshal.PtrToStructure<Action>(actionPtr);
|
||||
FreeNativeAction(ref action);
|
||||
}
|
||||
Marshal.FreeHGlobal(edge.actions);
|
||||
}
|
||||
}
|
||||
|
||||
private static Trajectory ConvertToNativeTrajectory(OrderTrajectoryData trajectoryData)
|
||||
{
|
||||
var trajectory = new Trajectory
|
||||
{
|
||||
degree = trajectoryData.Degree,
|
||||
knotVector_count = (nuint)trajectoryData.KnotVector.Count,
|
||||
controlPoints_count = (nuint)trajectoryData.ControlPoints.Count
|
||||
};
|
||||
|
||||
// Allocate knot vector
|
||||
if (trajectoryData.KnotVector.Count > 0)
|
||||
{
|
||||
trajectory.knotVector = Marshal.AllocHGlobal(sizeof(double) * trajectoryData.KnotVector.Count);
|
||||
Marshal.Copy(trajectoryData.KnotVector.ToArray(), 0, trajectory.knotVector, trajectoryData.KnotVector.Count);
|
||||
}
|
||||
else
|
||||
{
|
||||
trajectory.knotVector = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// Allocate control points
|
||||
if (trajectoryData.ControlPoints.Count > 0)
|
||||
{
|
||||
int cpSize = Marshal.SizeOf<ControlPoint>();
|
||||
trajectory.controlPoints = Marshal.AllocHGlobal(cpSize * trajectoryData.ControlPoints.Count);
|
||||
|
||||
for (int i = 0; i < trajectoryData.ControlPoints.Count; i++)
|
||||
{
|
||||
var cpData = trajectoryData.ControlPoints[i];
|
||||
var cp = new ControlPoint
|
||||
{
|
||||
x = cpData.X,
|
||||
y = cpData.Y,
|
||||
weight = cpData.Weight
|
||||
};
|
||||
IntPtr cpPtr = IntPtr.Add(trajectory.controlPoints, i * cpSize);
|
||||
Marshal.StructureToPtr(cp, cpPtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
trajectory.controlPoints = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return trajectory;
|
||||
}
|
||||
|
||||
private static void FreeNativeTrajectory(ref Trajectory trajectory)
|
||||
{
|
||||
if (trajectory.knotVector != IntPtr.Zero) Marshal.FreeHGlobal(trajectory.knotVector);
|
||||
if (trajectory.controlPoints != IntPtr.Zero) Marshal.FreeHGlobal(trajectory.controlPoints);
|
||||
}
|
||||
|
||||
private static Action ConvertToNativeAction(OrderActionData actionData)
|
||||
{
|
||||
var action = new Action
|
||||
{
|
||||
actionType = Marshal.StringToHGlobalAnsi(actionData.ActionType),
|
||||
actionId = Marshal.StringToHGlobalAnsi(actionData.ActionId),
|
||||
actionDescription = Marshal.StringToHGlobalAnsi(actionData.ActionDescription),
|
||||
blockingType = Marshal.StringToHGlobalAnsi(actionData.BlockingType),
|
||||
actionParameters_count = (nuint)actionData.ActionParameters.Count
|
||||
};
|
||||
|
||||
// Convert parameters
|
||||
if (actionData.ActionParameters.Count > 0)
|
||||
{
|
||||
int paramSize = Marshal.SizeOf<ActionParameter>();
|
||||
action.actionParameters = Marshal.AllocHGlobal(paramSize * actionData.ActionParameters.Count);
|
||||
|
||||
for (int i = 0; i < actionData.ActionParameters.Count; i++)
|
||||
{
|
||||
var paramData = actionData.ActionParameters[i];
|
||||
var param = new ActionParameter
|
||||
{
|
||||
key = Marshal.StringToHGlobalAnsi(paramData.Key),
|
||||
value = Marshal.StringToHGlobalAnsi(paramData.Value)
|
||||
};
|
||||
IntPtr paramPtr = IntPtr.Add(action.actionParameters, i * paramSize);
|
||||
Marshal.StructureToPtr(param, paramPtr, false);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
action.actionParameters = IntPtr.Zero;
|
||||
}
|
||||
|
||||
return action;
|
||||
}
|
||||
|
||||
private static void FreeNativeAction(ref Action action)
|
||||
{
|
||||
if (action.actionType != IntPtr.Zero) Marshal.FreeHGlobal(action.actionType);
|
||||
if (action.actionId != IntPtr.Zero) Marshal.FreeHGlobal(action.actionId);
|
||||
if (action.actionDescription != IntPtr.Zero) Marshal.FreeHGlobal(action.actionDescription);
|
||||
if (action.blockingType != IntPtr.Zero) Marshal.FreeHGlobal(action.blockingType);
|
||||
|
||||
if (action.actionParameters != IntPtr.Zero)
|
||||
{
|
||||
int paramSize = Marshal.SizeOf<ActionParameter>();
|
||||
for (int i = 0; i < (int)action.actionParameters_count; i++)
|
||||
{
|
||||
IntPtr paramPtr = IntPtr.Add(action.actionParameters, i * paramSize);
|
||||
ActionParameter param = Marshal.PtrToStructure<ActionParameter>(paramPtr);
|
||||
if (param.key != IntPtr.Zero) Marshal.FreeHGlobal(param.key);
|
||||
if (param.value != IntPtr.Zero) Marshal.FreeHGlobal(param.value);
|
||||
}
|
||||
Marshal.FreeHGlobal(action.actionParameters);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,515 @@
|
||||
using System.Runtime.InteropServices;
|
||||
// using RobotNet10.Shared.Sensor;
|
||||
|
||||
namespace RobotNet10.RobotApp.Navigation;
|
||||
|
||||
/// <summary>
|
||||
/// P/Invoke declarations for Navigation C API
|
||||
/// </summary>
|
||||
public static class NavigationNativeInterface
|
||||
{
|
||||
// Library path
|
||||
private const string LibraryPath = "/usr/local/lib/libnav_c_api.so";
|
||||
|
||||
#region String Management
|
||||
|
||||
/// <summary>
|
||||
/// Free a string allocated by the library
|
||||
/// </summary>
|
||||
/// <param name="str">String to free</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void nav_c_api_free_string(IntPtr str);
|
||||
|
||||
#endregion
|
||||
|
||||
#region State Conversion
|
||||
|
||||
/// <summary>
|
||||
/// Convert a State enum to its string representation
|
||||
/// </summary>
|
||||
/// <param name="state">Enum value of NavigationState</param>
|
||||
/// <returns>String representation (caller must free with nav_c_api_free_string)</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr navigation_state_to_string(NavigationState state);
|
||||
|
||||
#endregion
|
||||
|
||||
#region Header Functions
|
||||
|
||||
/// <summary>
|
||||
/// Create a new header
|
||||
/// </summary>
|
||||
/// <param name="frame_id">Frame id</param>
|
||||
/// <returns>Header</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
public static extern Header header_create([MarshalAs(UnmanagedType.LPUTF8Str)] string frame_id);
|
||||
|
||||
/// <summary>
|
||||
/// Set data for a header
|
||||
/// </summary>
|
||||
/// <param name="seq">Sequence</param>
|
||||
/// <param name="sec">Second</param>
|
||||
/// <param name="nsec">Nanosecond</param>
|
||||
/// <param name="frame_id">Frame id</param>
|
||||
/// <returns>Header</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
|
||||
public static extern Header header_set_data(
|
||||
uint seq,
|
||||
uint sec,
|
||||
uint nsec,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string frame_id);
|
||||
|
||||
/// <summary>
|
||||
/// Create a new time
|
||||
/// </summary>
|
||||
/// <returns>Time</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern Time time_create();
|
||||
|
||||
#endregion
|
||||
|
||||
#region Helper Functions
|
||||
|
||||
/// <summary>
|
||||
/// Creates a target pose by offsetting a given 2D pose along its heading direction
|
||||
/// </summary>
|
||||
/// <param name="pose_x">X coordinate of the original pose</param>
|
||||
/// <param name="pose_y">Y coordinate of the original pose</param>
|
||||
/// <param name="pose_theta">Heading angle in radians</param>
|
||||
/// <param name="frame_id">The coordinate frame ID (null-terminated string)</param>
|
||||
/// <param name="offset_distance">Distance to offset along heading (positive = forward, negative = backward)</param>
|
||||
/// <param name="out_goal">Output parameter for the offset pose</param>
|
||||
/// <returns>true on success, false on failure</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_offset_goal_2d(
|
||||
double pose_x, double pose_y, double pose_theta,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string frame_id,
|
||||
double offset_distance,
|
||||
out PoseStamped out_goal);
|
||||
|
||||
/// <summary>
|
||||
/// Creates an offset target pose from a given PoseStamped
|
||||
/// </summary>
|
||||
/// <param name="in_pose">Input pose</param>
|
||||
/// <param name="offset_distance">Distance to offset along heading direction</param>
|
||||
/// <param name="out_goal">Output parameter for the offset pose</param>
|
||||
/// <returns>true on success, false on failure</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_offset_goal_stamped(
|
||||
ref PoseStamped in_pose,
|
||||
double offset_distance,
|
||||
out PoseStamped out_goal);
|
||||
|
||||
#endregion
|
||||
|
||||
#region Navigation Handle Management
|
||||
|
||||
/// <summary>
|
||||
/// Create a new navigation instance
|
||||
/// </summary>
|
||||
/// <returns>Navigation handle, or IntPtr.Zero on failure</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr navigation_create();
|
||||
|
||||
/// <summary>
|
||||
/// Destroy a navigation instance
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle to destroy</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_destroy(IntPtr handle);
|
||||
|
||||
#endregion
|
||||
|
||||
#region TF Listener Management
|
||||
|
||||
/// <summary>
|
||||
/// Create a TF listener instance
|
||||
/// </summary>
|
||||
/// <returns>TF listener handle, or IntPtr.Zero on failure</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr tf_listener_create();
|
||||
|
||||
/// <summary>
|
||||
/// Destroy a TF listener instance
|
||||
/// </summary>
|
||||
/// <param name="handle">TF listener handle to destroy</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void tf_listener_destroy(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Inject a static transform into the TF buffer
|
||||
/// </summary>
|
||||
/// <param name="tf_handle">TF listener handle</param>
|
||||
/// <param name="parent_frame">Parent frame id (e.g. "map")</param>
|
||||
/// <param name="child_frame">Child frame id (e.g. "base_link")</param>
|
||||
/// <param name="x">Translation x (meters)</param>
|
||||
/// <param name="y">Translation y (meters)</param>
|
||||
/// <param name="z">Translation z (meters)</param>
|
||||
/// <param name="qx">Rotation quaternion x</param>
|
||||
/// <param name="qy">Rotation quaternion y</param>
|
||||
/// <param name="qz">Rotation quaternion z</param>
|
||||
/// <param name="qw">Rotation quaternion w</param>
|
||||
/// <returns>true on success, false on failure</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool tf_listener_set_static_transform(
|
||||
IntPtr tf_handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string parent_frame,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string child_frame,
|
||||
double x, double y, double z,
|
||||
double qx, double qy, double qz, double qw);
|
||||
|
||||
#endregion
|
||||
|
||||
#region Navigation Interface Methods
|
||||
|
||||
/// <summary>
|
||||
/// Initialize the navigation system
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="tf_handle">TF listener handle</param>
|
||||
/// <returns>true on success, false on failure</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);
|
||||
|
||||
/// <summary>
|
||||
/// Set the robot's footprint (outline shape)
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="points">Array of points representing the footprint polygon</param>
|
||||
/// <param name="point_count">Number of points in the array</param>
|
||||
/// <returns>true on success, false on failure</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_set_robot_footprint(
|
||||
IntPtr handle,
|
||||
IntPtr points,
|
||||
nuint point_count);
|
||||
|
||||
/// <summary>
|
||||
/// Get the robot's footprint (outline shape)
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="out_points">Output array of points (allocated by library, free with navigation_free_points)</param>
|
||||
/// <param name="out_count">Output number of points in the array</param>
|
||||
/// <returns>true on success, false on failure</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_get_robot_footprint(
|
||||
IntPtr handle,
|
||||
out IntPtr out_points,
|
||||
out nuint out_count);
|
||||
|
||||
/// <summary>
|
||||
/// Free a points array allocated by navigation_get_robot_footprint
|
||||
/// </summary>
|
||||
/// <param name="points">Pointer to point array</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_points(IntPtr points);
|
||||
|
||||
/// <summary>
|
||||
/// Send a goal for the robot to navigate to
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="goal">Target pose in the global frame</param>
|
||||
/// <param name="xy_goal_tolerance">Acceptable error in X/Y (meters)</param>
|
||||
/// <param name="yaw_goal_tolerance">Acceptable angular error (radians)</param>
|
||||
/// <returns>true if goal was accepted and sent successfully</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_move_to(
|
||||
IntPtr handle,
|
||||
PoseStamped goal);
|
||||
|
||||
/// <summary>
|
||||
/// Send a goal for the robot to navigate to with order
|
||||
/// Note: This function may not be available in all C API versions
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="order">Order handle</param>
|
||||
/// <param name="goal">Target pose in the global frame</param>
|
||||
/// <param name="xy_goal_tolerance">Acceptable error in X/Y (meters)</param>
|
||||
/// <param name="yaw_goal_tolerance">Acceptable angular error (radians)</param>
|
||||
/// <returns>true if goal was accepted and sent successfully</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_move_to_order(IntPtr handle, Order order, PoseStamped goal);
|
||||
|
||||
/// <summary>
|
||||
/// Send a docking goal to a predefined marker
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="marker">Marker name or ID (null-terminated string)</param>
|
||||
/// <param name="goal">Target pose for docking</param>
|
||||
/// <param name="xy_goal_tolerance">Acceptable XY error (meters)</param>
|
||||
/// <param name="yaw_goal_tolerance">Acceptable heading error (radians)</param>
|
||||
/// <returns>true if docking command succeeded</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_dock_to(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string marker,
|
||||
PoseStamped goal);
|
||||
|
||||
/// <summary>
|
||||
/// Move straight toward the target position
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="goal">Target pose</param>
|
||||
/// <param name="xy_goal_tolerance">Acceptable positional error (meters)</param>
|
||||
/// <returns>true if command issued successfully</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_move_straight_to(
|
||||
IntPtr handle,
|
||||
double distance);
|
||||
|
||||
/// <summary>
|
||||
/// Rotate in place to align with target orientation
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="goal">Pose containing desired heading (only Z-axis used)</param>
|
||||
/// <param name="yaw_goal_tolerance">Acceptable angular error (radians)</param>
|
||||
/// <returns>true if rotation command was sent successfully</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_rotate_to(
|
||||
IntPtr handle,
|
||||
PoseStamped goal);
|
||||
|
||||
/// <summary>
|
||||
/// Pause the robot's movement
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_pause(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Resume motion after a pause
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_resume(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Cancel the current goal and stop the robot
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_cancel(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Send limited linear velocity command
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="linear_x">Linear velocity in X direction</param>
|
||||
/// <param name="linear_y">Linear velocity in Y direction</param>
|
||||
/// <param name="linear_z">Linear velocity in Z direction</param>
|
||||
/// <returns>true if the command was accepted</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_set_twist_linear(
|
||||
IntPtr handle,
|
||||
double linear_x, double linear_y, double linear_z);
|
||||
|
||||
/// <summary>
|
||||
/// Send limited angular velocity command
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="angular_x">Angular velocity around X axis</param>
|
||||
/// <param name="angular_y">Angular velocity around Y axis</param>
|
||||
/// <param name="angular_z">Angular velocity around Z axis</param>
|
||||
/// <returns>true if the command was accepted</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_set_twist_angular(
|
||||
IntPtr handle,
|
||||
double angular_x, double angular_y, double angular_z);
|
||||
|
||||
/// <summary>
|
||||
/// Get the robot's pose as a PoseStamped
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="out_pose">Output parameter with the robot's current pose</param>
|
||||
/// <returns>true if pose was successfully retrieved</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_get_robot_pose_stamped(
|
||||
IntPtr handle,
|
||||
ref PoseStamped out_pose);
|
||||
|
||||
/// <summary>
|
||||
/// Get the robot's pose as a 2D pose
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="out_pose">Output parameter with the robot's current 2D pose</param>
|
||||
/// <returns>true if pose was successfully retrieved</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_get_robot_pose_2d(
|
||||
IntPtr handle,
|
||||
ref Pose2D out_pose);
|
||||
|
||||
/// <summary>
|
||||
/// Get the robot's current twist
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="out_twist">Output parameter with the robot's current twist</param>
|
||||
/// <returns>true if twist was successfully retrieved</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_get_twist(
|
||||
IntPtr handle,
|
||||
ref Twist2DStamped ref_twist);
|
||||
|
||||
/// <summary>
|
||||
/// Get navigation feedback
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="out_feedback">Output parameter with navigation feedback</param>
|
||||
/// <returns>true if feedback was successfully retrieved</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_get_feedback(
|
||||
IntPtr handle,
|
||||
ref NavFeedback out_feedback);
|
||||
|
||||
/// <summary>
|
||||
/// Get global planner data from navigation system
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_get_global_data(
|
||||
IntPtr handle,
|
||||
ref PlannerDataOutput out_data);
|
||||
|
||||
/// <summary>
|
||||
/// Get local planner data from navigation system
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_get_local_data(
|
||||
IntPtr handle,
|
||||
ref PlannerDataOutput out_data);
|
||||
|
||||
/// <summary>
|
||||
/// Free navigation feedback structure
|
||||
/// </summary>
|
||||
/// <param name="feedback">Feedback structure to free</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern void navigation_free_feedback(ref NavFeedback feedback);
|
||||
|
||||
/// <summary>
|
||||
/// Free an occupancy grid handle
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_occupancy_grid(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Free an occupancy grid update handle
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_occupancy_grid_update(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Free a laser scan handle
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_laser_scan(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Free an odometry handle
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_odometry(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Free a path2d handle
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_path2d(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Free a polygon stamped handle
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_polygon_stamped(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Free an order handle
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_order(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Free an array of named occupancy grids
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_occupancy_grids(IntPtr maps, nuint count);
|
||||
|
||||
/// <summary>
|
||||
/// Free an array of named laser scans
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void navigation_free_named_laser_scans(IntPtr scans, nuint count);
|
||||
|
||||
// [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
// [return: MarshalAs(UnmanagedType.Bool)]
|
||||
// public static extern void navigation_free_planner_data(PlannerDataOutputHandle handle);
|
||||
|
||||
/// <summary>
|
||||
/// Add a static map to the navigation system
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="map_name">Name of the map</param>
|
||||
/// <param name="occupancy_grid">Occupancy grid handle</param>
|
||||
/// <returns>true if the map was added successfully</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_add_static_map(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string map_name,
|
||||
OccupancyGrid occupancy_grid);
|
||||
|
||||
/// <summary>
|
||||
/// Add a laser scan to the navigation system
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="laser_scan_name">Name of the laser scan</param>
|
||||
/// <param name="laser_scan">Laser scan handle</param>
|
||||
/// <returns>true if the laser scan was added successfully</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_add_laser_scan(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string laser_scan_name,
|
||||
LaserScan laser_scan);
|
||||
|
||||
/// <summary>
|
||||
/// Add an odometry to the navigation system
|
||||
/// </summary>
|
||||
/// <param name="handle">Navigation handle</param>
|
||||
/// <param name="odometry_name">Name of the odometry</param>
|
||||
/// <param name="odometry">Odometry handle</param>
|
||||
/// <returns>true if the odometry was added successfully</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
public static extern bool navigation_add_odometry(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string odometry_name,
|
||||
Odometry odometry);
|
||||
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
[return: MarshalAs(UnmanagedType.I1)]
|
||||
public static extern bool navigation_dock_to_order(
|
||||
IntPtr handle,
|
||||
Order order,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string marker,
|
||||
PoseStamped goal);
|
||||
#endregion
|
||||
}
|
||||
|
||||
@@ -0,0 +1,202 @@
|
||||
{
|
||||
"headerId": 1,
|
||||
"timestamp": "2026-02-28T16:01:25.308Z",
|
||||
"version": "0.0.1",
|
||||
"manufacturer": "phenikaaX",
|
||||
"serialNumber": "T800Fake",
|
||||
"orderId": "c92ec33e-ec04-4521-8255-af8ae46b7f31",
|
||||
"orderUpdateId": 1,
|
||||
"zoneSetId": "maze",
|
||||
"nodes": [
|
||||
{
|
||||
"nodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"sequenceId": 0,
|
||||
"nodeDescription": "RobotCurrentNode",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 8.266311457600798,
|
||||
"y": 3.645018708246215,
|
||||
"theta": -0.04770435896493348,
|
||||
"allowedDeviationXY": 0,
|
||||
"allowedDeviationTheta": 0,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"sequenceId": 1,
|
||||
"nodeDescription": "Node172",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 13.051897126315499,
|
||||
"y": 3.416552077732615,
|
||||
"theta": -0.04770435896493223,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "7abba653-c3fb-42ec-21ab-08de5340bfbf",
|
||||
"sequenceId": 2,
|
||||
"nodeDescription": "Node175",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 14.796447573406823,
|
||||
"y": 5.098507632838125,
|
||||
"theta": 1.5721664299784024,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "a713a913-b201-414a-21ac-08de5340bfbf",
|
||||
"sequenceId": 3,
|
||||
"nodeDescription": "Node176",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 14.796447573406823,
|
||||
"y": 7.16397917617426,
|
||||
"theta": 1.5707963267948966,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
],
|
||||
"edges": [
|
||||
{
|
||||
"edgeId": "522212fa-74d5-4f00-9a00-23e092593bcc",
|
||||
"sequenceId": 0,
|
||||
"edgeDescription": "86312653-08b2-4556-bb84-674805905bed - 3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"released": true,
|
||||
"startNodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"endNodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"maxSpeed": 0.5,
|
||||
"maxHeight": 0,
|
||||
"minHeight": 0,
|
||||
"orientation": 0,
|
||||
"orientationType": "",
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"length": 0,
|
||||
"trajectory": {
|
||||
"degree": 1,
|
||||
"knotVector": [
|
||||
0,
|
||||
0,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"controlPoints": [
|
||||
{
|
||||
"x": 8.266311457600798,
|
||||
"y": 3.645018708246215,
|
||||
"weight": 1
|
||||
},
|
||||
{
|
||||
"x": 13.051897126315499,
|
||||
"y": 3.416552077732615,
|
||||
"weight": 1
|
||||
}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"edgeId": "56ba53c3-e20e-4abb-a017-08de5340bfc5",
|
||||
"sequenceId": 1,
|
||||
"edgeDescription": "Node172 - Node175",
|
||||
"released": true,
|
||||
"startNodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"endNodeId": "7abba653-c3fb-42ec-21ab-08de5340bfbf",
|
||||
"maxSpeed": 0.3,
|
||||
"maxHeight": 1,
|
||||
"minHeight": 0.1,
|
||||
"orientation": 0,
|
||||
"orientationType": "",
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"length": 2.747,
|
||||
"trajectory": {
|
||||
"degree": 2,
|
||||
"knotVector": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
1,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"controlPoints": [
|
||||
{
|
||||
"x": 13.051897126315499,
|
||||
"y": 3.416552077732615,
|
||||
"weight": 1
|
||||
},
|
||||
{
|
||||
"x": 14.807552259123684,
|
||||
"y": 3.432804775308984,
|
||||
"weight": 1
|
||||
},
|
||||
{
|
||||
"x": 14.796447573406823,
|
||||
"y": 5.098507632838125,
|
||||
"weight": 1
|
||||
}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"edgeId": "b17623aa-1ea5-4036-a018-08de5340bfc5",
|
||||
"sequenceId": 2,
|
||||
"edgeDescription": "Node175 - Node176",
|
||||
"released": true,
|
||||
"startNodeId": "7abba653-c3fb-42ec-21ab-08de5340bfbf",
|
||||
"endNodeId": "a713a913-b201-414a-21ac-08de5340bfbf",
|
||||
"maxSpeed": 0.5,
|
||||
"maxHeight": 1,
|
||||
"minHeight": 0.1,
|
||||
"orientation": 0,
|
||||
"orientationType": "",
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"length": 2.065,
|
||||
"trajectory": {
|
||||
"degree": 1,
|
||||
"knotVector": [
|
||||
0,
|
||||
0,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"controlPoints": [
|
||||
{
|
||||
"x": 14.796447573406823,
|
||||
"y": 5.098507632838125,
|
||||
"weight": 1
|
||||
},
|
||||
{
|
||||
"x": 14.796447573406823,
|
||||
"y": 7.16397917617426,
|
||||
"weight": 1
|
||||
}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
using System.Text.Json;
|
||||
using RobotNet.VDA5050.Type;
|
||||
|
||||
namespace RobotNet10.RobotApp.Navigation;
|
||||
|
||||
// Use aliases so Node/Edge resolve to VDA5050 (this file is in Navigation namespace which has native Node/Edge structs).
|
||||
using VdaNode = RobotNet.VDA5050.Order.Node;
|
||||
using VdaEdge = RobotNet.VDA5050.Order.Edge;
|
||||
using VdaTrajectory = RobotNet.VDA5050.Order.Trajectory;
|
||||
using VdaOrderMsg = RobotNet.VDA5050.Order.OrderMsg;
|
||||
|
||||
/// <summary>
|
||||
/// Converts VDA5050 order (Node[]/Edge[] from MQTT) to OrderData for Navigation C API (MoveToOrder).
|
||||
/// </summary>
|
||||
public static class VDA5050ToOrderDataConverter
|
||||
{
|
||||
/// <summary>
|
||||
/// Convert VDA5050 nodes and edges (e.g. from OrderMsg received via MQTT) to OrderData for navigation.
|
||||
/// </summary>
|
||||
public static OrderData ToOrderData(VdaNode[] nodes, VdaEdge[] edges, VdaOrderMsg? orderMsg = null)
|
||||
{
|
||||
var data = new OrderData
|
||||
{
|
||||
HeaderId = (int)(orderMsg?.HeaderId ?? 0),
|
||||
Timestamp = orderMsg?.Timestamp.ToString("o") ?? DateTime.UtcNow.ToString("o"),
|
||||
Version = orderMsg?.Version ?? "2.1.0",
|
||||
Manufacturer = orderMsg?.Manufacturer ?? string.Empty,
|
||||
SerialNumber = orderMsg?.SerialNumber ?? string.Empty,
|
||||
OrderId = orderMsg?.OrderId ?? string.Empty,
|
||||
OrderUpdateId = (uint)(orderMsg?.OrderUpdateId ?? 0),
|
||||
ZoneSetId = orderMsg?.ZoneSetId,
|
||||
Nodes = new List<OrderNodeData>(nodes.Length),
|
||||
Edges = new List<OrderEdgeData>(edges.Length)
|
||||
};
|
||||
|
||||
foreach (var n in nodes)
|
||||
data.Nodes.Add(ToOrderNodeData(n));
|
||||
foreach (var e in edges)
|
||||
data.Edges.Add(ToOrderEdgeData(e));
|
||||
|
||||
return data;
|
||||
}
|
||||
|
||||
private static OrderNodeData ToOrderNodeData(VdaNode n)
|
||||
{
|
||||
var node = new OrderNodeData
|
||||
{
|
||||
NodeId = n.NodeId,
|
||||
SequenceId = n.SequenceId,
|
||||
NodeDescription = n.NodeDescription ?? string.Empty,
|
||||
Released = n.Released,
|
||||
NodePosition = n.NodePosition != null ? new OrderNodePositionData
|
||||
{
|
||||
X = n.NodePosition.X,
|
||||
Y = n.NodePosition.Y,
|
||||
Theta = n.NodePosition.Theta,
|
||||
AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
|
||||
AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
|
||||
MapId = n.NodePosition.MapId,
|
||||
MapDescription = n.NodePosition.MapDescription
|
||||
} : null,
|
||||
Actions = new List<OrderActionData>()
|
||||
};
|
||||
return node;
|
||||
}
|
||||
|
||||
private static OrderEdgeData ToOrderEdgeData(VdaEdge e)
|
||||
{
|
||||
var edge = new OrderEdgeData
|
||||
{
|
||||
EdgeId = e.EdgeId,
|
||||
SequenceId = e.SequenceId,
|
||||
EdgeDescription = e.EdgeDescription,
|
||||
Released = e.Released,
|
||||
StartNodeId = e.StartNodeId,
|
||||
EndNodeId = e.EndNodeId,
|
||||
MaxSpeed = e.MaxSpeed,
|
||||
MaxHeight = e.MaxHeight,
|
||||
MinHeight = e.MinHeight,
|
||||
Orientation = e.Orientation ?? 0,
|
||||
OrientationType = e.OrientationType.HasValue ? JsonSerializer.SerializeToElement((int)e.OrientationType.Value) : null,
|
||||
Direction = e.Direction ?? string.Empty,
|
||||
RotationAllowed = e.RotationAllowed,
|
||||
MaxRotationSpeed = e.MaxRotationSpeed,
|
||||
Length = e.Length ?? 0,
|
||||
Trajectory = e.Trajectory != null ? ToOrderTrajectoryData(e.Trajectory) : null,
|
||||
Actions = new List<OrderActionData>()
|
||||
};
|
||||
return edge;
|
||||
}
|
||||
|
||||
private static OrderTrajectoryData? ToOrderTrajectoryData(VdaTrajectory? t)
|
||||
{
|
||||
if (t == null) return null;
|
||||
return new OrderTrajectoryData
|
||||
{
|
||||
Degree = (uint)t.Degree,
|
||||
KnotVector = t.KnotVector?.ToList() ?? new List<double>(),
|
||||
ControlPoints = t.ControlPoints?.Select(cp => new OrderControlPointData
|
||||
{
|
||||
X = cp.X,
|
||||
Y = cp.Y,
|
||||
Weight = cp.Weight ?? 1.0
|
||||
}).ToList() ?? new List<OrderControlPointData>()
|
||||
};
|
||||
}
|
||||
}
|
||||
123
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/a.md
Normal file
123
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/a.md
Normal file
@@ -0,0 +1,123 @@
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order_data \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"x": 1.4,
|
||||
"y": 0.8,
|
||||
"yaw": 1.57,
|
||||
"frame_id": "map",
|
||||
"xy_tolerance": 0.3,
|
||||
"yaw_tolerance": 50,
|
||||
"order": {
|
||||
"headerId": 1,
|
||||
"timestamp": "2026-02-28T16:01:25.308Z",
|
||||
"version": "0.0.1",
|
||||
"manufacturer": "phenikaaX",
|
||||
"serialNumber": "T800Fake",
|
||||
"orderId": "c92ec33e-ec04-4521-8255-af8ae46b7f31",
|
||||
"orderUpdateId": 1,
|
||||
"zoneSetId": "maze",
|
||||
|
||||
"nodes": [
|
||||
{
|
||||
"nodeId": "node0",
|
||||
"sequenceId": 0,
|
||||
"nodeDescription": "Node0",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": -0.0,
|
||||
"y": 0.0,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0,
|
||||
"allowedDeviationTheta": 0,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
|
||||
{
|
||||
"nodeId": "node1",
|
||||
"sequenceId": 1,
|
||||
"nodeDescription": "Node1",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
|
||||
{
|
||||
"nodeId": "node2",
|
||||
"sequenceId": 2,
|
||||
"nodeDescription": "Node2",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 0.7,
|
||||
"y": 0.0,
|
||||
"theta": 1.57,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
],
|
||||
|
||||
"edges": [
|
||||
|
||||
{
|
||||
"edgeId": "edge0",
|
||||
"sequenceId": 0,
|
||||
"edgeDescription": "node0-node1",
|
||||
"released": true,
|
||||
"startNodeId": "node0",
|
||||
"endNodeId": "node1",
|
||||
"maxSpeed": 0.5,
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"trajectory": {
|
||||
"degree": 1,
|
||||
"knotVector": [0,0,1,1],
|
||||
"controlPoints": [
|
||||
{"x": -0.0, "y": 0.0, "weight": 1},
|
||||
{"x": 0.0, "y": 0.0, "weight": 1}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
|
||||
{
|
||||
"edgeId": "edge1",
|
||||
"sequenceId": 1,
|
||||
"edgeDescription": "node1-node2",
|
||||
"released": true,
|
||||
"startNodeId": "node1",
|
||||
"endNodeId": "node2",
|
||||
"maxSpeed": 0.5,
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"trajectory": {
|
||||
"degree": 2,
|
||||
"knotVector": [0,0,0,1,1,1],
|
||||
"controlPoints": [
|
||||
{"x": 0.0, "y": 0.0, "weight": 1.0},
|
||||
{"x": 0.7, "y": 0.0, "weight": 0.707},
|
||||
{"x": 1.4, "y": 0.8, "weight": 1.0}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
|
||||
]
|
||||
|
||||
}
|
||||
}'
|
||||
125
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/b.md
Normal file
125
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/b.md
Normal file
@@ -0,0 +1,125 @@
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order_data \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"x": -2.0,
|
||||
"y": 0.3,
|
||||
"yaw": 0.0,
|
||||
"frame_id": "map",
|
||||
"xy_tolerance": 0.3,
|
||||
"yaw_tolerance": 50,
|
||||
"order": {
|
||||
"headerId": 1,
|
||||
"timestamp": "2026-02-28T16:01:25.308Z",
|
||||
"version": "0.0.1",
|
||||
"manufacturer": "phenikaaX",
|
||||
"serialNumber": "T800Fake",
|
||||
"orderId": "reverse-order",
|
||||
"orderUpdateId": 1,
|
||||
"zoneSetId": "maze",
|
||||
|
||||
"nodes": [
|
||||
|
||||
{
|
||||
"nodeId": "node2",
|
||||
"sequenceId": 0,
|
||||
"nodeDescription": "Node2",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 1.4,
|
||||
"y": 0.8,
|
||||
"theta": 1.57,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
|
||||
{
|
||||
"nodeId": "node1",
|
||||
"sequenceId": 1,
|
||||
"nodeDescription": "Node1",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 0.0,
|
||||
"y": 0.3,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
|
||||
{
|
||||
"nodeId": "node0",
|
||||
"sequenceId": 2,
|
||||
"nodeDescription": "Node0",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": -2.0,
|
||||
"y": 0.3,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0,
|
||||
"allowedDeviationTheta": 0,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
|
||||
],
|
||||
|
||||
"edges": [
|
||||
|
||||
{
|
||||
"edgeId": "edge1-rev",
|
||||
"sequenceId": 0,
|
||||
"edgeDescription": "node2-node1",
|
||||
"released": true,
|
||||
"startNodeId": "node2",
|
||||
"endNodeId": "node1",
|
||||
"maxSpeed": 0.5,
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"trajectory": {
|
||||
"degree": 2,
|
||||
"knotVector": [0,0,0,1,1,1],
|
||||
"controlPoints": [
|
||||
{"x": 1.4, "y": 0.8, "weight": 1.0},
|
||||
{"x": 0.7, "y": 0.3, "weight": 0.707},
|
||||
{"x": 0.0, "y": 0.3, "weight": 1.0}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
|
||||
{
|
||||
"edgeId": "edge0-rev",
|
||||
"sequenceId": 1,
|
||||
"edgeDescription": "node1-node0",
|
||||
"released": true,
|
||||
"startNodeId": "node1",
|
||||
"endNodeId": "node0",
|
||||
"maxSpeed": 0.5,
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"trajectory": {
|
||||
"degree": 1,
|
||||
"knotVector": [0,0,1,1],
|
||||
"controlPoints": [
|
||||
{"x": 0.0, "y": 0.3, "weight": 1},
|
||||
{"x": -2.0, "y": 0.3, "weight": 1}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
|
||||
]
|
||||
|
||||
}
|
||||
}'
|
||||
104
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/c.MD
Normal file
104
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/c.MD
Normal file
@@ -0,0 +1,104 @@
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order_data \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"x": 5.0,
|
||||
"y": 0.05,
|
||||
"yaw": 0.0,
|
||||
"frame_id": "map",
|
||||
"xy_tolerance": 0.1,
|
||||
"yaw_tolerance": 0.1,
|
||||
"order":
|
||||
{
|
||||
"headerId": 6,
|
||||
"timestamp": "2026-03-12T10:34:35.292Z",
|
||||
"version": "2.1.0",
|
||||
"manufacturer": "PhenikaaX",
|
||||
"serialNumber": "I150",
|
||||
"orderId": "777f6600-580e-40c5-ad27-d69703e3f980",
|
||||
"orderUpdateId": 0,
|
||||
"zoneSetId": null,
|
||||
"nodes": [
|
||||
{
|
||||
"nodeId": "fc9fbda7-8c1b-4457-6112-08de79d9211c",
|
||||
"sequenceId": 0,
|
||||
"nodeDescription": "",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 0.05,
|
||||
"y": 0.05,
|
||||
"theta": 0,
|
||||
"allowedDeviationXY": null,
|
||||
"allowedDeviationTheta": null,
|
||||
"mapId": "3e0c7bc7-d561-4409-5c99-08de79d846ff",
|
||||
"mapDescription": null
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "6233663f-c64f-43fd-6113-08de79d9211c",
|
||||
"sequenceId": 2,
|
||||
"nodeDescription": "",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 5.03,
|
||||
"y": 0.05,
|
||||
"theta": 0,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 0.5,
|
||||
"mapId": "3e0c7bc7-d561-4409-5c99-08de79d846ff",
|
||||
"mapDescription": null
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
],
|
||||
"edges": [
|
||||
{
|
||||
"edgeId": "504a8c38-ea15-425a-4ba8-08de79d92122",
|
||||
"sequenceId": 1,
|
||||
"edgeDescription": null,
|
||||
"released": true,
|
||||
"startNodeId": "fc9fbda7-8c1b-4457-6112-08de79d9211c",
|
||||
"endNodeId": "6233663f-c64f-43fd-6113-08de79d9211c",
|
||||
"maxSpeed": null,
|
||||
"maxHeight": null,
|
||||
"minHeight": null,
|
||||
"orientation": 0,
|
||||
"orientationType": 1,
|
||||
"direction": "",
|
||||
"rotationAllowed": null,
|
||||
"maxRotationSpeed": null,
|
||||
"trajectory": {
|
||||
"degree": 2,
|
||||
"knotVector": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
1,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"controlPoints": [
|
||||
{
|
||||
"x": 0.05,
|
||||
"y": 0.05,
|
||||
"weight": 1
|
||||
},
|
||||
{
|
||||
"x": 2.43,
|
||||
"y": 0.917,
|
||||
"weight": 0.9
|
||||
},
|
||||
{
|
||||
"x": 5.0,
|
||||
"y": 0.05,
|
||||
"weight": 1
|
||||
}
|
||||
]
|
||||
},
|
||||
"length": 4.978658199310303,
|
||||
"corridor": null,
|
||||
"actions": []
|
||||
}
|
||||
]
|
||||
}
|
||||
}'
|
||||
82
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/d.md
Normal file
82
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/d.md
Normal file
@@ -0,0 +1,82 @@
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order_data \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"x": 5.0,
|
||||
"y": 0.05,
|
||||
"yaw": 0,
|
||||
"frame_id": "map",
|
||||
"xy_tolerance": 0.3,
|
||||
"yaw_tolerance": 50,
|
||||
"order": {
|
||||
"headerId": 1,
|
||||
"timestamp": "2026-02-28T16:01:25.308Z",
|
||||
"version": "0.0.1",
|
||||
"manufacturer": "phenikaaX",
|
||||
"serialNumber": "T800Fake",
|
||||
"orderId": "c92ec33e-ec04-4521-8255-af8ae46b7f31",
|
||||
"orderUpdateId": 1,
|
||||
"zoneSetId": "maze",
|
||||
"nodes": [
|
||||
{
|
||||
"nodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"sequenceId": 0,
|
||||
"nodeDescription": "Node0",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 0.05,
|
||||
"y": 0.05,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0,
|
||||
"allowedDeviationTheta": 0,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"sequenceId": 1,
|
||||
"nodeDescription": "Node1",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 5.0,
|
||||
"y": 0.05,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
],
|
||||
"edges": [
|
||||
{
|
||||
"edgeId": "13312fds-fdsfsdaaf",
|
||||
"sequenceId": 1,
|
||||
"edgeDescription": "86312653-08b2-4556-bb84-674805905bed - 3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"released": true,
|
||||
"startNodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"endNodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"maxSpeed": 0.5,
|
||||
"maxHeight": 0,
|
||||
"minHeight": 0,
|
||||
"orientation": 0,
|
||||
"orientationType": "",
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"length": 0,
|
||||
"trajectory": {
|
||||
"degree": 1,
|
||||
"knotVector": [0, 0, 1, 1],
|
||||
"controlPoints": [
|
||||
{"x": 0.05, "y": 0.05, "weight": 1.0},
|
||||
{"x": 5.0, "y": 0.05, "weight": 1.0}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
]
|
||||
}
|
||||
}'
|
||||
82
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/e.md
Normal file
82
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Navigation/e.md
Normal file
@@ -0,0 +1,82 @@
|
||||
curl -k -X POST https://localhost:7002/api/navigation/move_to_order_data \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{
|
||||
"x": 5.0,
|
||||
"y": 0.05,
|
||||
"yaw": 0,
|
||||
"frame_id": "map",
|
||||
"xy_tolerance": 0.3,
|
||||
"yaw_tolerance": 50,
|
||||
"order": {
|
||||
"headerId": 1,
|
||||
"timestamp": "2026-02-28T16:01:25.308Z",
|
||||
"version": "0.0.1",
|
||||
"manufacturer": "phenikaaX",
|
||||
"serialNumber": "T800Fake",
|
||||
"orderId": "c92ec33e-ec04-4521-8255-af8ae46b7f31",
|
||||
"orderUpdateId": 1,
|
||||
"zoneSetId": "maze",
|
||||
"nodes": [
|
||||
{
|
||||
"nodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"sequenceId": 0,
|
||||
"nodeDescription": "Node0",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 0.05,
|
||||
"y": 0.05,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0,
|
||||
"allowedDeviationTheta": 0,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
},
|
||||
{
|
||||
"nodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"sequenceId": 1,
|
||||
"nodeDescription": "Node1",
|
||||
"released": true,
|
||||
"nodePosition": {
|
||||
"x": 5.0,
|
||||
"y": 0.05,
|
||||
"theta": 0.0,
|
||||
"allowedDeviationXY": 0.3,
|
||||
"allowedDeviationTheta": 50,
|
||||
"mapId": "448039b7-23d2-4a70-a2d2-08dd61dadd33",
|
||||
"mapDescription": ""
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
],
|
||||
"edges": [
|
||||
{
|
||||
"edgeId": "13312fds-fdsfsdaaf",
|
||||
"sequenceId": 1,
|
||||
"edgeDescription": "86312653-08b2-4556-bb84-674805905bed - 3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"released": true,
|
||||
"startNodeId": "86312653-08b2-4556-bb84-674805905bed",
|
||||
"endNodeId": "3726205f-0140-454d-21a8-08de5340bfbf",
|
||||
"maxSpeed": 0.5,
|
||||
"maxHeight": 0,
|
||||
"minHeight": 0,
|
||||
"orientation": 0,
|
||||
"orientationType": "",
|
||||
"direction": "Both",
|
||||
"rotationAllowed": true,
|
||||
"maxRotationSpeed": 0.5,
|
||||
"length": 0,
|
||||
"trajectory": {
|
||||
"degree": 1,
|
||||
"knotVector": [0, 0, 1, 1],
|
||||
"controlPoints": [
|
||||
{"x": 0.05, "y": 0.05, "weight": 1.0},
|
||||
{"x": 5.0, "y": 0.05, "weight": 1.0}
|
||||
]
|
||||
},
|
||||
"actions": []
|
||||
}
|
||||
]
|
||||
}
|
||||
}'
|
||||
Reference in New Issue
Block a user