740 lines
33 KiB
C#
740 lines
33 KiB
C#
using Microsoft.AspNetCore.Mvc;
|
|
using System.Text.Json;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.RobotApp.Navigation;
|
|
|
|
namespace RobotNet10.RobotApp.Xloc;
|
|
|
|
public static class XlocApiEndpoints
|
|
{
|
|
// XLOC Control API
|
|
public static IEndpointRouteBuilder MapXlocApiEndpoints(this IEndpointRouteBuilder app)
|
|
{
|
|
var xlocApi = app.MapGroup("/api/xloc").DisableAntiforgery();
|
|
|
|
// Mapping control
|
|
xlocApi.MapPost("/mapping/start", ([FromServices] XlocIntegrationService service) => {
|
|
return service.StartMapping()
|
|
? Results.Ok(new { status = "success", message = "Mapping started" })
|
|
: Results.BadRequest(new { status = "error", message = "Failed to start mapping" });
|
|
});
|
|
|
|
xlocApi.MapPost("/mapping/stop", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
|
var body = await request.ReadFromJsonAsync<Dictionary<string, string>>();
|
|
var mapFile = body?["map_file_path"] ?? "/home/mic-733ao/.local/share/xloc/resources/maps";
|
|
|
|
return service.StopMapping(mapFile)
|
|
? Results.Ok(new { status = "success", message = "Mapping stopped", map_file = mapFile })
|
|
: Results.BadRequest(new { status = "error", message = "Failed to stop mapping" });
|
|
});
|
|
|
|
// Localization control
|
|
xlocApi.MapPost("/localization/start", ([FromServices] XlocIntegrationService service) => {
|
|
return service.StartLocalization()
|
|
? Results.Ok(new { status = "success", message = "Localization started" })
|
|
: Results.BadRequest(new { status = "error", message = "Failed to start localization" });
|
|
});
|
|
|
|
xlocApi.MapPost("/localization/stop", ([FromServices] XlocIntegrationService service) => {
|
|
return service.StopLocalization()
|
|
? Results.Ok(new { status = "success", message = "Localization stopped" })
|
|
: Results.BadRequest(new { status = "error", message = "Failed to stop localization" });
|
|
});
|
|
|
|
// Update map control (only allowed when in localization mode)
|
|
xlocApi.MapPost("/update-map/start", ([FromServices] XlocIntegrationService service) => {
|
|
return service.StartUpdateMap()
|
|
? Results.Ok(new { status = "success", message = "Map update started" })
|
|
: Results.BadRequest(new { status = "error", message = "Failed to start map update. Robot must be in localization mode." });
|
|
});
|
|
|
|
xlocApi.MapPost("/update-map/stop", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
|
var body = await request.ReadFromJsonAsync<Dictionary<string, object>>();
|
|
bool saveUpdatedMap = true;
|
|
if (body != null && body.TryGetValue("save_updated_map", out var saveVal))
|
|
{
|
|
if (saveVal is bool saveBool)
|
|
saveUpdatedMap = saveBool;
|
|
else if (saveVal is JsonElement jsonElem && jsonElem.ValueKind == JsonValueKind.True)
|
|
saveUpdatedMap = true;
|
|
else if (saveVal is JsonElement jsonElem2 && jsonElem2.ValueKind == JsonValueKind.False)
|
|
saveUpdatedMap = false;
|
|
}
|
|
|
|
return service.StopUpdateMap(saveUpdatedMap)
|
|
? Results.Ok(new { status = "success", message = "Map update stopped", save_updated_map = saveUpdatedMap })
|
|
: Results.BadRequest(new { status = "error", message = "Failed to stop map update" });
|
|
});
|
|
|
|
// Reset SLAM error
|
|
xlocApi.MapPost("/reset-error", ([FromServices] XlocIntegrationService service) => {
|
|
return service.ResetSlamError()
|
|
? Results.Ok(new { status = "success", message = "SLAM error reset successfully" })
|
|
: Results.BadRequest(new { status = "error", message = "Failed to reset SLAM error" });
|
|
});
|
|
|
|
// Initial pose control
|
|
xlocApi.MapPost("/pose/initial", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
|
var body = await request.ReadFromJsonAsync<Dictionary<string, double>>();
|
|
if (body == null)
|
|
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
|
|
|
if (!body.TryGetValue("x", out var x) || !body.TryGetValue("y", out var y))
|
|
return Results.BadRequest(new { status = "error", message = "x and y are required" });
|
|
|
|
var z = body.TryGetValue("z", out var zVal) ? zVal : 0.0;
|
|
var roll = body.TryGetValue("roll", out var rollVal) ? rollVal : 0.0;
|
|
var pitch = body.TryGetValue("pitch", out var pitchVal) ? pitchVal : 0.0;
|
|
var yaw = body.TryGetValue("yaw", out var yawVal) ? yawVal : 0.0;
|
|
|
|
return service.SetInitialPose(x, y, z, roll, pitch, yaw)
|
|
? Results.Ok(new { status = "success", message = "Initial pose set", position = new { x, y, z }, orientation = new { roll, pitch, yaw } })
|
|
: Results.BadRequest(new { status = "error", message = "Failed to set initial pose" });
|
|
});
|
|
|
|
// Map management
|
|
// xlocApi.MapPost("/map/activate", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
|
// var body = await request.ReadFromJsonAsync<Dictionary<string, string>>();
|
|
// var mapFile = body?["map_file_path"];
|
|
|
|
// if (string.IsNullOrEmpty(mapFile))
|
|
// return Results.BadRequest(new { status = "error", message = "map_file_path required" });
|
|
|
|
// // UI may send only the map folder name (e.g. "map_20260307_103646").
|
|
// // Resolve it to an absolute pbstream path so native XLOC doesn't depend on HOME defaults (e.g. /root vs /home/...).
|
|
// var resolved = XlocPaths.ResolvePbstreamPath(mapFile);
|
|
// if (resolved == null)
|
|
// return Results.BadRequest(new
|
|
// {
|
|
// status = "error",
|
|
// message = "Failed to resolve pbstream path for map_file_path. " +
|
|
// "Expected either an absolute .pbstream file, or a map folder name under: " +
|
|
// XlocPaths.GetMapsDirectory()
|
|
// });
|
|
|
|
// return service.ActivateMap(resolved)
|
|
// ? Results.Ok(new { status = "success", message = "Map activated", map_file = resolved })
|
|
// : Results.BadRequest(new { status = "error", message = $"Failed to activate map pbstream: {resolved}" });
|
|
// });
|
|
|
|
xlocApi.MapPost("/map/activate", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
|
var body = await request.ReadFromJsonAsync<Dictionary<string, string>>();
|
|
var mapFile = body?["map_file_path"];
|
|
|
|
if (string.IsNullOrEmpty(mapFile))
|
|
return Results.BadRequest(new { status = "error", message = "map_file_path required" });
|
|
|
|
return service.ActivateMap(mapFile)
|
|
? Results.Ok(new { status = "success", message = "Map activated", map_file = mapFile })
|
|
: Results.BadRequest(new { status = "error", message = "Failed to activate map" });
|
|
});
|
|
|
|
// Grid map endpoints
|
|
xlocApi.MapGet("/gridmap/static", ([FromServices] XlocIntegrationService service, [FromServices] ILogger<Program> logger, [FromQuery] bool reload = false) => {
|
|
// Log when map is being requested (to track if it's called after initial pose)
|
|
// logger.LogWarning("🔍 API /gridmap/static called (reload={Reload})", reload);
|
|
|
|
var grid = service.GetStaticGridMap(reload);
|
|
if (grid == null)
|
|
return Results.NotFound(new { status = "error", message = "No static grid map available" });
|
|
|
|
// Prefer origin from active map's YAML (Map frame in World frame); native often returns (0,0,0)
|
|
double ox = grid.Origin.X, oy = grid.Origin.Y, oz = grid.Origin.Z;
|
|
var diag = service.GetDiagnostics();
|
|
if (!string.IsNullOrEmpty(diag?.CurrentActiveMap))
|
|
{
|
|
var (yx, yy, yz) = ReadOriginFromMapYaml(diag.CurrentActiveMap);
|
|
ox = yx; oy = yy; oz = yz;
|
|
}
|
|
|
|
//logger.LogWarning("🔍 API /gridmap/static returning origin: ({X}, {Y}, {Z})", ox, oy, oz);
|
|
|
|
return Results.Ok(new {
|
|
status = "success",
|
|
resolution = grid.Resolution,
|
|
width = grid.Width,
|
|
height = grid.Height,
|
|
origin = new {
|
|
x = ox,
|
|
y = oy,
|
|
z = oz,
|
|
qx = grid.Origin.Qx,
|
|
qy = grid.Origin.Qy,
|
|
qz = grid.Origin.Qz,
|
|
qw = grid.Origin.Qw
|
|
},
|
|
data = Convert.ToBase64String(grid.Data),
|
|
frameId = grid.FrameId
|
|
});
|
|
});
|
|
|
|
xlocApi.MapGet("/gridmap/online", ([FromServices] XlocIntegrationService service) => {
|
|
var grid = service.GetOnlineGridMap();
|
|
if (grid == null)
|
|
return Results.NotFound(new { status = "error", message = "No online grid map available" });
|
|
|
|
// CRITICAL: Use origin directly from online grid map to ensure alignment with LIDAR
|
|
// Do NOT override with YAML origin during mapping, as online map origin may change
|
|
// and must match the actual grid data for proper rendering
|
|
double ox = grid.Origin.X, oy = grid.Origin.Y, oz = grid.Origin.Z;
|
|
|
|
// Only use YAML origin for static maps, not for online maps during mapping
|
|
// Online map origin should come directly from xloc_get_online_grid_map()
|
|
// var diag = service.GetDiagnostics();
|
|
// if (!string.IsNullOrEmpty(diag?.CurrentActiveMap))
|
|
// {
|
|
// var (yx, yy, yz) = ReadOriginFromMapYaml(diag.CurrentActiveMap);
|
|
// ox = yx; oy = yy; oz = yz;
|
|
// }
|
|
|
|
return Results.Ok(new {
|
|
status = "success",
|
|
resolution = grid.Resolution,
|
|
width = grid.Width,
|
|
height = grid.Height,
|
|
origin = new {
|
|
x = ox,
|
|
y = oy,
|
|
z = oz,
|
|
qx = grid.Origin.Qx,
|
|
qy = grid.Origin.Qy,
|
|
qz = grid.Origin.Qz,
|
|
qw = grid.Origin.Qw
|
|
},
|
|
data = Convert.ToBase64String(grid.Data),
|
|
frameId = grid.FrameId
|
|
});
|
|
});
|
|
|
|
// List available maps
|
|
xlocApi.MapGet("/maps/list", () => {
|
|
try
|
|
{
|
|
var mapsDir = XlocPaths.GetMapsDirectory();
|
|
if (!Directory.Exists(mapsDir))
|
|
return Results.Ok(new { status = "success", maps = Array.Empty<string>() });
|
|
|
|
var maps = Directory.GetDirectories(mapsDir)
|
|
.Select(dir => Path.GetFileName(dir))
|
|
.Where(name => !string.IsNullOrEmpty(name) && name != "tmp")
|
|
.OrderBy(name => name)
|
|
.ToArray();
|
|
|
|
return Results.Ok(new { status = "success", maps });
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
return Results.BadRequest(new { status = "error", message = ex.Message });
|
|
}
|
|
});
|
|
|
|
// Load map from folder
|
|
xlocApi.MapGet("/maps/load/{mapName}", (string mapName) => {
|
|
try
|
|
{
|
|
var mapsDir = XlocPaths.GetMapsDirectory();
|
|
var mapFolder = Path.Combine(mapsDir, mapName);
|
|
|
|
if (!Directory.Exists(mapFolder))
|
|
return Results.NotFound(new { status = "error", message = $"Map '{mapName}' not found" });
|
|
|
|
// Find YAML file
|
|
var yamlFiles = Directory.GetFiles(mapFolder, "*.yaml");
|
|
if (yamlFiles.Length == 0)
|
|
return Results.BadRequest(new { status = "error", message = "YAML file not found" });
|
|
|
|
var yamlFile = yamlFiles[0];
|
|
var yamlContent = File.ReadAllText(yamlFile);
|
|
|
|
// Parse YAML (simple parsing)
|
|
float resolution = 0.05f;
|
|
double originX = 0.0, originY = 0.0, originZ = 0.0;
|
|
string? imageFile = null;
|
|
|
|
foreach (var line in yamlContent.Split('\n'))
|
|
{
|
|
var trimmed = line.Trim();
|
|
if (trimmed.StartsWith("resolution:"))
|
|
{
|
|
if (float.TryParse(trimmed.Substring("resolution:".Length).Trim(), out float res))
|
|
resolution = res;
|
|
}
|
|
else if (trimmed.StartsWith("origin:"))
|
|
{
|
|
var originStr = trimmed.Substring("origin:".Length).Trim();
|
|
if (originStr.StartsWith("[") && originStr.EndsWith("]"))
|
|
{
|
|
var coords = originStr.Substring(1, originStr.Length - 2).Split(',');
|
|
if (coords.Length >= 1) double.TryParse(coords[0].Trim(), out originX);
|
|
if (coords.Length >= 2) double.TryParse(coords[1].Trim(), out originY);
|
|
if (coords.Length >= 3) double.TryParse(coords[2].Trim(), out originZ);
|
|
}
|
|
}
|
|
else if (trimmed.StartsWith("image:"))
|
|
{
|
|
imageFile = trimmed.Substring("image:".Length).Trim();
|
|
}
|
|
}
|
|
|
|
// Find PGM file - always prefer .pgm over .png from YAML
|
|
string? pgmFile = null;
|
|
|
|
// First try to find .pgm file directly
|
|
var pgmFiles = Directory.GetFiles(mapFolder, "*.pgm");
|
|
if (pgmFiles.Length > 0)
|
|
{
|
|
pgmFile = pgmFiles[0];
|
|
}
|
|
else if (imageFile != null)
|
|
{
|
|
// If YAML specifies .png, try replacing extension with .pgm
|
|
var pgmFromYaml = Path.Combine(mapFolder, Path.ChangeExtension(imageFile, ".pgm"));
|
|
if (File.Exists(pgmFromYaml))
|
|
pgmFile = pgmFromYaml;
|
|
else
|
|
{
|
|
// Last resort: use image file from YAML (might be .png)
|
|
var imageFromYaml = Path.Combine(mapFolder, imageFile);
|
|
if (File.Exists(imageFromYaml) && imageFile.EndsWith(".pgm", StringComparison.OrdinalIgnoreCase))
|
|
pgmFile = imageFromYaml;
|
|
}
|
|
}
|
|
|
|
if (pgmFile == null)
|
|
return Results.BadRequest(new { status = "error", message = "PGM file not found" });
|
|
|
|
// Read PGM file and convert to occupancy grid
|
|
var (width, height, data) = ReadPgmFile(pgmFile);
|
|
|
|
// Convert to base64
|
|
var dataBase64 = Convert.ToBase64String(data);
|
|
|
|
return Results.Ok(new {
|
|
status = "success",
|
|
mapName,
|
|
width,
|
|
height,
|
|
resolution,
|
|
origin = new {
|
|
x = originX,
|
|
y = originY,
|
|
z = originZ,
|
|
qx = 0.0,
|
|
qy = 0.0,
|
|
qz = 0.0,
|
|
qw = 1.0
|
|
},
|
|
data = dataBase64
|
|
});
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
return Results.BadRequest(new { status = "error", message = ex.Message });
|
|
}
|
|
});
|
|
|
|
// Download map folder as .zip (single top-level folder matching map name inside the archive on re-import)
|
|
xlocApi.MapGet("/maps/download/{mapName}", (string mapName) => XlocMapFileEndpoints.DownloadMapAsZip(mapName));
|
|
|
|
// Import map from .zip into maps directory
|
|
xlocApi.MapPost("/maps/import", async (HttpRequest request) => await XlocMapFileEndpoints.ImportMapFromZipAsync(request));
|
|
|
|
// Delete map folder (explicit path avoids conflicting with /maps/list, /maps/load/...)
|
|
xlocApi.MapDelete("/maps/delete/{mapName}", (string mapName) => XlocMapFileEndpoints.DeleteMapFolder(mapName));
|
|
|
|
// Map upload endpoint (kept for backward compatibility)
|
|
xlocApi.MapPost("/map/upload", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
|
try
|
|
{
|
|
if (!request.HasFormContentType)
|
|
return Results.BadRequest(new { status = "error", message = "Request must be multipart/form-data" });
|
|
|
|
var form = await request.ReadFormAsync();
|
|
var file = form.Files["mapFile"];
|
|
if (file == null || file.Length == 0)
|
|
return Results.BadRequest(new { status = "error", message = "No file uploaded" });
|
|
|
|
// Validate PNG
|
|
if (!file.FileName.EndsWith(".png", StringComparison.OrdinalIgnoreCase))
|
|
return Results.BadRequest(new { status = "error", message = "File must be a PNG image" });
|
|
|
|
// Parse origin and resolution from form
|
|
if (!float.TryParse(form["resolution"], out float resolution))
|
|
resolution = 0.05f; // default 5cm
|
|
|
|
if (!double.TryParse(form["originX"], out double originX))
|
|
originX = 0.0;
|
|
if (!double.TryParse(form["originY"], out double originY))
|
|
originY = 0.0;
|
|
if (!double.TryParse(form["originZ"], out double originZ))
|
|
originZ = 0.0;
|
|
if (!double.TryParse(form["originQx"], out double originQx))
|
|
originQx = 0.0;
|
|
if (!double.TryParse(form["originQy"], out double originQy))
|
|
originQy = 0.0;
|
|
if (!double.TryParse(form["originQz"], out double originQz))
|
|
originQz = 0.0;
|
|
if (!double.TryParse(form["originQw"], out double originQw))
|
|
originQw = 1.0;
|
|
|
|
// Save file to temp directory
|
|
var uploadsDir = Path.Combine(Path.GetTempPath(), "xloc_maps");
|
|
Directory.CreateDirectory(uploadsDir);
|
|
var filePath = Path.Combine(uploadsDir, file.FileName);
|
|
|
|
using (var stream = File.Create(filePath))
|
|
{
|
|
await file.CopyToAsync(stream);
|
|
}
|
|
|
|
// Activate map with origin
|
|
var originPose = new { x = originX, y = originY, z = originZ, qx = originQx, qy = originQy, qz = originQz, qw = originQw };
|
|
|
|
// Note: xloc may need the map in a specific format, so we save the PNG and metadata
|
|
// The actual activation might need conversion to xloc's map format
|
|
var metadata = new {
|
|
filePath,
|
|
resolution,
|
|
origin = originPose
|
|
};
|
|
|
|
return Results.Ok(new {
|
|
status = "success",
|
|
message = "Map uploaded successfully",
|
|
filePath,
|
|
resolution,
|
|
origin = originPose
|
|
});
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
return Results.BadRequest(new { status = "error", message = ex.Message });
|
|
}
|
|
});
|
|
|
|
// Pose data
|
|
xlocApi.MapGet("/pose/current", ([FromServices] XlocIntegrationService service) => {
|
|
var pose = service.GetCurrentPose();
|
|
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 }
|
|
}
|
|
})
|
|
: Results.NotFound(new { status = "error", message = "No pose available" });
|
|
});
|
|
|
|
xlocApi.MapGet("/pose/current2d", ([FromServices] XlocIntegrationService service) => {
|
|
var pose = service.GetCurrentPose2D();
|
|
return pose.HasValue
|
|
? Results.Ok(new {
|
|
status = "success",
|
|
pose = new { x = pose.Value.x, y = pose.Value.y, yaw = pose.Value.yaw }
|
|
})
|
|
: Results.NotFound(new { status = "error", message = "No pose available" });
|
|
});
|
|
|
|
// Diagnostics data
|
|
xlocApi.MapGet("/diagnostics", ([FromServices] XlocIntegrationService service) => {
|
|
var diagnostics = service.GetDiagnostics();
|
|
return diagnostics != null
|
|
? Results.Ok(new {
|
|
status = "success",
|
|
diagnostics = new {
|
|
header = new {
|
|
seq = diagnostics.HeaderSeq,
|
|
stamp = new {
|
|
sec = diagnostics.HeaderStampSec,
|
|
nsec = diagnostics.HeaderStampNsec
|
|
},
|
|
frameId = diagnostics.HeaderFrameId
|
|
},
|
|
xlocState = diagnostics.XlocState,
|
|
stateString = diagnostics.StateString,
|
|
currentActiveMap = diagnostics.CurrentActiveMap,
|
|
reliability = diagnostics.Reliability,
|
|
matchingScore = diagnostics.MatchingScore
|
|
}
|
|
})
|
|
: Results.NotFound(new { status = "error", message = "No diagnostics available" });
|
|
});
|
|
|
|
// Laser scan data (sampled at 1 degree intervals)
|
|
xlocApi.MapGet("/laser/scan", ([FromServices] XlocIntegrationService service) => {
|
|
var laserData = service.GetSampledLaserScan();
|
|
if (laserData == null || laserData.Count == 0)
|
|
return Results.NotFound(new { status = "error", message = "No laser scan available" });
|
|
|
|
var points = laserData.Select(p => new { angle = p.angle, range = p.range }).ToArray();
|
|
|
|
return Results.Ok(new {
|
|
status = "success",
|
|
pointCount = points.Length,
|
|
points = points,
|
|
timestamp = DateTime.UtcNow
|
|
});
|
|
});
|
|
|
|
// All 3 lidars combined with mode support (full = all points, minimal = 120 sampled points)
|
|
xlocApi.MapGet("/laser/scan/all", (
|
|
[FromServices] XlocIntegrationService service,
|
|
[FromQuery] string? mode1 = "minimal",
|
|
[FromQuery] string? mode2 = "minimal",
|
|
[FromQuery] string? mode3 = "minimal") => {
|
|
|
|
// Get laser data based on mode (full = all points, minimal = ~120 sampled points)
|
|
var laser1 = mode1 == "full" ? service.GetFullLaserScan() : service.GetSampledLaserScan();
|
|
var laser2 = mode2 == "full" ? service.GetFullLaserScan2() : service.GetSampledLaserScan2();
|
|
var laser3 = mode3 == "full" ? service.GetFullLaserScan3() : service.GetSampledLaserScan3();
|
|
|
|
return Results.Ok(new {
|
|
status = "success",
|
|
timestamp = DateTime.UtcNow,
|
|
lidar1 = laser1 != null ? new {
|
|
pointCount = laser1.Count,
|
|
points = laser1.Select(p => new { angle = p.angle, range = p.range }).ToArray()
|
|
} : null,
|
|
lidar2 = laser2 != null ? new {
|
|
pointCount = laser2.Count,
|
|
points = laser2.Select(p => new { angle = p.angle, range = p.range }).ToArray()
|
|
} : null,
|
|
lidar3 = laser3 != null ? new {
|
|
pointCount = laser3.Count,
|
|
points = laser3.Select(p => new { angle = p.angle, range = p.range }).ToArray()
|
|
} : null
|
|
});
|
|
});
|
|
|
|
// Velocity data (OdomVel and CmdVel)
|
|
xlocApi.MapGet("/velocity", ([FromServices] OdometryService odometryService, [FromServices] ManualControlService manualControlService, [FromServices] PS5ControllerService ps5ControllerService, [FromServices] NavigationIntegrationService navigationService) => {
|
|
try
|
|
{
|
|
// Get odometry velocity directly from OdometryService (raw encoder odometry, not EKF filtered)
|
|
var odom = odometryService.CurrentOdometry;
|
|
double odomLinearVel = odom.Twist.Twist.Linear.X;
|
|
double odomAngularVel = odom.Twist.Twist.Angular.Z;
|
|
|
|
// Get command velocity from the active control service
|
|
// Priority: PS5Controller > ManualControl > Navigation twist
|
|
double cmdLinearVel = 0.0;
|
|
double cmdAngularVel = 0.0;
|
|
|
|
// Check PS5Controller first (if active)
|
|
if (ps5ControllerService.State == PS5ControllerState.Active)
|
|
{
|
|
var ps5Twist = ps5ControllerService.CurrentTwist;
|
|
cmdLinearVel = ps5Twist.Linear.X;
|
|
cmdAngularVel = ps5Twist.Angular.Z;
|
|
}
|
|
// Otherwise check ManualControl (if active)
|
|
else if (manualControlService.State == ManualControlState.Active)
|
|
{
|
|
var manualTwist = manualControlService.CurrentTwist;
|
|
cmdLinearVel = manualTwist.Linear.X;
|
|
cmdAngularVel = manualTwist.Angular.Z;
|
|
}
|
|
// Otherwise try to get navigation twist (from GetTwist in navigation system)
|
|
else
|
|
{
|
|
var navTwist = navigationService.GetTwist();
|
|
if (navTwist.HasValue)
|
|
{
|
|
cmdLinearVel = navTwist.Value.x;
|
|
cmdAngularVel = navTwist.Value.theta;
|
|
}
|
|
else
|
|
{
|
|
// Fallback: last known value from ManualControl
|
|
var manualTwist = manualControlService.CurrentTwist;
|
|
cmdLinearVel = manualTwist.Linear.X;
|
|
cmdAngularVel = manualTwist.Angular.Z;
|
|
}
|
|
}
|
|
|
|
return Results.Ok(new {
|
|
status = "success",
|
|
odomVel = new {
|
|
linear = odomLinearVel,
|
|
angular = odomAngularVel
|
|
},
|
|
cmdVel = new {
|
|
linear = cmdLinearVel,
|
|
angular = cmdAngularVel
|
|
}
|
|
});
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
return Results.BadRequest(new { status = "error", message = ex.Message });
|
|
}
|
|
});
|
|
|
|
// Diagnostics endpoint
|
|
// DISABLED: GetDiagnostics method removed from XlocIntegrationService
|
|
/*
|
|
app.MapGet("/api/xloc/diagnostics", ([FromServices] XlocIntegrationService xlocService) =>
|
|
{
|
|
var diagnostics = xlocService.GetDiagnostics();
|
|
if (diagnostics.HasValue)
|
|
{
|
|
return Results.Ok(new { status = "success", diagnostics = new {
|
|
state = diagnostics.Value.state,
|
|
map_name = diagnostics.Value.mapName,
|
|
reliability = diagnostics.Value.reliability,
|
|
matching_score = diagnostics.Value.matchingScore
|
|
}});
|
|
}
|
|
return Results.Ok(new { status = "error", message = "No diagnostics available" });
|
|
});
|
|
*/
|
|
|
|
return app;
|
|
}
|
|
|
|
// Helper: read origin (Map frame in World frame) from active map's YAML
|
|
private static (double x, double y, double z) ReadOriginFromMapYaml(string mapName)
|
|
{
|
|
try
|
|
{
|
|
var mapsDir = XlocPaths.GetMapsDirectory();
|
|
var folderName = Path.GetFileName(mapName.TrimEnd(Path.DirectorySeparatorChar, '/'));
|
|
if (string.IsNullOrEmpty(folderName)) return (0, 0, 0);
|
|
var mapFolder = Path.Combine(mapsDir, folderName);
|
|
if (!Directory.Exists(mapFolder)) return (0, 0, 0);
|
|
var yamlFiles = Directory.GetFiles(mapFolder, "*.yaml");
|
|
if (yamlFiles.Length == 0) return (0, 0, 0);
|
|
var content = File.ReadAllText(yamlFiles[0]);
|
|
double ox = 0, oy = 0, oz = 0;
|
|
foreach (var line in content.Split('\n'))
|
|
{
|
|
var t = line.Trim();
|
|
if (!t.StartsWith("origin:")) continue;
|
|
var s = t.Substring("origin:".Length).Trim();
|
|
if (s.StartsWith("[") && s.EndsWith("]"))
|
|
{
|
|
var parts = s.Substring(1, s.Length - 2).Split(',');
|
|
if (parts.Length >= 1) double.TryParse(parts[0].Trim(), out ox);
|
|
if (parts.Length >= 2) double.TryParse(parts[1].Trim(), out oy);
|
|
if (parts.Length >= 3) double.TryParse(parts[2].Trim(), out oz);
|
|
}
|
|
break;
|
|
}
|
|
return (ox, oy, oz);
|
|
}
|
|
catch { return (0, 0, 0); }
|
|
}
|
|
|
|
// Helper function to read PGM file
|
|
private static (uint width, uint height, byte[] data) ReadPgmFile(string pgmPath)
|
|
{
|
|
// Read entire file as bytes first
|
|
var allBytes = File.ReadAllBytes(pgmPath);
|
|
|
|
// Parse header manually to avoid StreamReader buffering issues
|
|
int pos = 0;
|
|
|
|
// Read magic number (P5\n)
|
|
string magic = "";
|
|
while (pos < allBytes.Length && allBytes[pos] != '\n')
|
|
{
|
|
magic += (char)allBytes[pos];
|
|
pos++;
|
|
}
|
|
pos++; // Skip newline
|
|
|
|
magic = magic.Trim();
|
|
if (magic != "P5")
|
|
throw new InvalidDataException($"Not a P5 PGM file, got: {magic}");
|
|
|
|
// Skip comments and read dimensions
|
|
uint width = 0, height = 0;
|
|
bool gotDimensions = false;
|
|
|
|
while (pos < allBytes.Length && !gotDimensions)
|
|
{
|
|
// Skip whitespace
|
|
while (pos < allBytes.Length && (allBytes[pos] == ' ' || allBytes[pos] == '\t' || allBytes[pos] == '\r' || allBytes[pos] == '\n'))
|
|
pos++;
|
|
|
|
// Check for comment
|
|
if (pos < allBytes.Length && allBytes[pos] == '#')
|
|
{
|
|
// Skip comment line
|
|
while (pos < allBytes.Length && allBytes[pos] != '\n')
|
|
pos++;
|
|
continue;
|
|
}
|
|
|
|
// Read dimensions line
|
|
string line = "";
|
|
while (pos < allBytes.Length && allBytes[pos] != '\n')
|
|
{
|
|
line += (char)allBytes[pos];
|
|
pos++;
|
|
}
|
|
pos++; // Skip newline
|
|
|
|
var parts = line.Trim().Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
|
|
if (parts.Length >= 2 && uint.TryParse(parts[0], out width) && uint.TryParse(parts[1], out height))
|
|
{
|
|
gotDimensions = true;
|
|
}
|
|
}
|
|
|
|
if (!gotDimensions || width == 0 || height == 0)
|
|
throw new InvalidDataException($"Invalid PGM dimensions: {width}x{height}");
|
|
|
|
// Read max value
|
|
while (pos < allBytes.Length && (allBytes[pos] == ' ' || allBytes[pos] == '\t' || allBytes[pos] == '\r' || allBytes[pos] == '\n'))
|
|
pos++;
|
|
|
|
string maxValStr = "";
|
|
while (pos < allBytes.Length && allBytes[pos] != '\n')
|
|
{
|
|
maxValStr += (char)allBytes[pos];
|
|
pos++;
|
|
}
|
|
pos++; // Skip newline
|
|
|
|
if (!int.TryParse(maxValStr.Trim(), out int maxVal))
|
|
throw new InvalidDataException($"Invalid max value in PGM: {maxValStr}");
|
|
|
|
// Read binary data
|
|
int dataSize = (int)(width * height);
|
|
if (pos + dataSize > allBytes.Length)
|
|
throw new InvalidDataException($"Not enough data: expected {dataSize} bytes at position {pos}, file size {allBytes.Length}");
|
|
|
|
var rawData = new byte[dataSize];
|
|
Array.Copy(allBytes, pos, rawData, 0, dataSize);
|
|
|
|
// Convert grayscale to occupancy grid format (int8_t):
|
|
// - 0 (black/occupied) -> 100
|
|
// - 255 (white/free) -> 0
|
|
// - 205 (unknown) -> -1 (255 in unsigned byte)
|
|
// IMPORTANT: Flip Y axis because PGM has row 0 at top, but ROS map has cell (0,0) at bottom-left
|
|
var occupancyData = new byte[dataSize];
|
|
for (int row = 0; row < height; row++)
|
|
{
|
|
for (int col = 0; col < width; col++)
|
|
{
|
|
// PGM index: row 0 = top
|
|
int pgmIndex = row * (int)width + col;
|
|
// ROS index: cell (x=col, y=0) at bottom → flip Y
|
|
int rosIndex = ((int)height - 1 - row) * (int)width + col;
|
|
|
|
var pixel = rawData[pgmIndex];
|
|
if (pixel >= 205) // Unknown (typically 205 in ROS maps)
|
|
occupancyData[rosIndex] = 255; // Will be converted to -1 in JS
|
|
else if (pixel < 128) // Dark = occupied
|
|
occupancyData[rosIndex] = 100; // Occupied
|
|
else // Light = free
|
|
occupancyData[rosIndex] = 0; // Free
|
|
}
|
|
}
|
|
|
|
return (width, height, occupancyData);
|
|
}
|
|
}
|