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