Initial commit
This commit is contained in:
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user