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(); 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 logger) => { try { logger.LogInformation("Received move_to_order request"); var body = await request.ReadFromJsonAsync(); 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(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 logger) => { try { logger.LogInformation("Received move_to_order_data request"); var body = await request.ReadFromJsonAsync(); 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(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(); 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(); 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(); 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(); 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(); 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; } }