1985 lines
86 KiB
C#
1985 lines
86 KiB
C#
using Microsoft.AspNetCore.Components.Authorization;
|
|
using Microsoft.AspNetCore.Identity;
|
|
using Microsoft.AspNetCore.Mvc;
|
|
using Microsoft.EntityFrameworkCore;
|
|
using MudBlazor.Services;
|
|
using NLog.Web;
|
|
using RobotNet10.CANOpen;
|
|
using RobotNet10.CustomConfiguration.Extensions;
|
|
using RobotNet10.MapManager.Extensions;
|
|
using RobotNet10.NavigationTune.Data;
|
|
using RobotNet10.NavigationTune.Execution;
|
|
using RobotNet10.NavigationTune.Extensions;
|
|
using RobotNet10.NavigationTune.Hubs;
|
|
using RobotNet10.RobotApp.Client;
|
|
using RobotNet10.RobotApp.Components;
|
|
using RobotNet10.RobotApp.Components.Account;
|
|
using RobotNet10.RobotApp.Data;
|
|
using RobotNet10.RobotApp.Detection;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Hubs;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Modules;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.RobotApp.Services;
|
|
using RobotNet10.RobotApp.Services.Navigation;
|
|
using RobotNet10.RobotApp.Services.Robot;
|
|
using RobotNet10.RobotApp.Services.NavigationMonitor;
|
|
using RobotNet10.RobotApp.Services.Simulation;
|
|
using RobotNet10.RobotApp.Navigation;
|
|
using RobotNet10.RobotApp.TF3;
|
|
using RobotNet10.RobotApp.Xloc;
|
|
using RobotNet10.ScriptEngine;
|
|
using RobotNet10.ScriptEngine.Helpers;
|
|
using System.Text.Json;
|
|
using Microsoft.AspNetCore.Http.Features;
|
|
|
|
// Print Ceres library information at startup
|
|
//CeresLibraryInfo.PrintLibraryInfo();
|
|
|
|
var builder = WebApplication.CreateBuilder(args);
|
|
|
|
var robotConfig = builder.Configuration["RobotConfig"];
|
|
if(string.IsNullOrEmpty(robotConfig) || !File.Exists(robotConfig))
|
|
{
|
|
throw new Exception("RobotConfig not found");
|
|
}
|
|
builder.Configuration.AddJsonFile(robotConfig);
|
|
|
|
// Add NLog
|
|
//builder.Logging.ClearProviders();
|
|
//builder.Logging.SetMinimumLevel(LogLevel.Trace);
|
|
builder.Host.UseNLog();
|
|
|
|
// Add services to the container.
|
|
builder.Services.AddRazorComponents()
|
|
.AddInteractiveServerComponents()
|
|
.AddInteractiveWebAssemblyComponents()
|
|
.AddAuthenticationStateSerialization();
|
|
|
|
builder.Services.AddCascadingAuthenticationState();
|
|
builder.Services.AddScoped<IdentityRedirectManager>();
|
|
builder.Services.AddScoped<AuthenticationStateProvider, IdentityRevalidatingAuthenticationStateProvider>();
|
|
|
|
builder.Services.AddAuthentication()
|
|
.AddBearerToken(IdentityConstants.BearerScheme);
|
|
|
|
var connectionString = builder.Configuration.GetConnectionString("DefaultConnection") ?? throw new InvalidOperationException("Connection string 'DefaultConnection' not found.");
|
|
builder.Services.AddDbContext<ApplicationDbContext>(options => options.UseSqlite(connectionString, b => b.MigrationsAssembly("RobotNet10.RobotApp").UseQuerySplittingBehavior(QuerySplittingBehavior.SplitQuery)));
|
|
builder.Services.AddDatabaseDeveloperPageExceptionFilter();
|
|
|
|
builder.Services.AddIdentity<ApplicationUser, IdentityRole>(options =>
|
|
{
|
|
options.SignIn.RequireConfirmedAccount = true;
|
|
options.Lockout.AllowedForNewUsers = false;
|
|
options.Password.RequireNonAlphanumeric = false;
|
|
options.Password.RequireUppercase = false;
|
|
options.Password.RequireLowercase = false;
|
|
options.Password.RequireDigit = false;
|
|
})
|
|
.AddEntityFrameworkStores<ApplicationDbContext>()
|
|
.AddSignInManager()
|
|
.AddDefaultTokenProviders();
|
|
|
|
builder.Services.AddSingleton<IEmailSender<ApplicationUser>, IdentityNoOpEmailSender>();
|
|
|
|
builder.Services.AddMudServices();
|
|
builder.Services.AddNavigationMenu();
|
|
|
|
builder.Services.Configure<FormOptions>(o =>
|
|
{
|
|
o.MultipartBodyLengthLimit = 200L * 1024 * 1024;
|
|
});
|
|
builder.WebHost.ConfigureKestrel(o =>
|
|
{
|
|
o.Limits.MaxRequestBodySize = 200L * 1024 * 1024;
|
|
});
|
|
|
|
// Add HttpClient for Blazor Server components (server-side calls into the same Kestrel app).
|
|
// BaseAddress must be loopback: using the browser's WAN/MOXA host from this machine often fails (hairpin NAT).
|
|
builder.Services.AddScoped(sp =>
|
|
{
|
|
var navigationManager = sp.GetRequiredService<Microsoft.AspNetCore.Components.NavigationManager>();
|
|
|
|
var handler = new HttpClientHandler
|
|
{
|
|
ServerCertificateCustomValidationCallback = (message, cert, chain, sslPolicyErrors) => true
|
|
};
|
|
|
|
var publicBase = new Uri(navigationManager.BaseUri);
|
|
var loopbackBase = new UriBuilder(publicBase) { Host = "127.0.0.1" }.Uri;
|
|
|
|
return new HttpClient(handler) { BaseAddress = loopbackBase };
|
|
});
|
|
|
|
builder.Services.AddControllers();
|
|
builder.Services.AddSignalR();
|
|
builder.Services.AddSingleton<DeviceHubContext>();
|
|
builder.Services.AddSingleton<OdometryHubContext>();
|
|
var scriptConnectionString = builder.Configuration.GetConnectionString("ScriptEngineConnection") ?? throw new InvalidOperationException("Connection string 'ScriptEngineConnection' not found.");
|
|
builder.Services.AddScriptEngine<ScriptEngineResource>(options => options.UseSqlite(scriptConnectionString, b => b.MigrationsAssembly("RobotNet10.RobotApp")));
|
|
|
|
var mapConnectionString = builder.Configuration.GetConnectionString("MapEditorConnection") ?? throw new InvalidOperationException("Connection string 'MapEditorConnection' not found.");
|
|
builder.Services.AddMapManager(builder.Configuration, options => options.UseSqlite(mapConnectionString, b => b.MigrationsAssembly("RobotNet10.RobotApp").UseQuerySplittingBehavior(QuerySplittingBehavior.SplitQuery)), "LayoutImage");
|
|
|
|
// var navTuneConnectionString = builder.Configuration.GetConnectionString("NavigationTuneConnection") ?? throw new InvalidOperationException("Connection string 'NavigationTuneConnection' not found.");
|
|
// builder.Services.AddNavigationTuningWithRobot(options => options.UseSqlite(navTuneConnectionString, b => b.MigrationsAssembly("RobotNet10.RobotApp").UseQuerySplittingBehavior(QuerySplittingBehavior.SplitQuery)));
|
|
|
|
// Register adapters for Navigation Tuning
|
|
// builder.Services.AddScoped<ILocalizationProvider>(sp =>
|
|
// {
|
|
// var localization = sp.GetRequiredService<RobotNet10.RobotApp.Interfaces.ILocalization>();
|
|
// return new LocalizationAdapter(
|
|
// () => localization.X,
|
|
// () => localization.Y,
|
|
// () => localization.Theta
|
|
// );
|
|
// });
|
|
|
|
builder.Services.AddScoped<IVelocityProvider>(sp =>
|
|
{
|
|
var velocityController = sp.GetRequiredService<IVelocityController>();
|
|
return new VelocityControllerAdapter(
|
|
() => velocityController.RawVelocity,
|
|
(linear, angular) => velocityController.SetVelocity(linear, angular),
|
|
() => velocityController.GetModelConfidence(),
|
|
acc => velocityController.SetAcceleration(acc),
|
|
dec => velocityController.SetDeceleration(dec)
|
|
);
|
|
});
|
|
|
|
// Add Custom Configuration
|
|
builder.Services.AddCustomConfiguration(builder.Configuration, "StorageConfig");
|
|
|
|
builder.Services.AddCanOpenManager();
|
|
builder.Services.AddSingleton<DeviceProvider>();
|
|
builder.Services.AddSingleton<IDeviceProvider>(sp => sp.GetRequiredService<DeviceProvider>());
|
|
builder.Services.AddHostedService(sp => sp.GetRequiredService<DeviceProvider>());
|
|
|
|
builder.Services.AddSingleton(typeof(RobotNet10.RobotApp.Services.Logger<>));
|
|
builder.Services.AddRobotSimulation();
|
|
|
|
// Core C API services (TF3 -> XLOC -> Navigation)
|
|
builder.Services.AddSingleton<TF3BufferManager>();
|
|
builder.Services.AddHostedService(sp => sp.GetRequiredService<TF3BufferManager>());
|
|
|
|
// XLOC integration is required by RobotLocalization.
|
|
builder.Services.AddSingleton<XlocIntegrationService>();
|
|
builder.Services.AddHostedService(sp => sp.GetRequiredService<XlocIntegrationService>());
|
|
builder.Services.AddHostedService<XlocAutoLocalizationHostedService>();
|
|
|
|
builder.Services.AddSingleton<NavigationIntegrationService>();
|
|
builder.Services.AddHostedService(sp => sp.GetRequiredService<NavigationIntegrationService>());
|
|
|
|
builder.Services.AddRobot();
|
|
|
|
// Motion services
|
|
builder.Services.AddSingleton<DifferentialDrive>();
|
|
builder.Services.AddSingleton<IInverseKinematics>(sp => sp.GetRequiredService<DifferentialDrive>());
|
|
builder.Services.AddSingleton<IOdometryEstimator>(sp => sp.GetRequiredService<DifferentialDrive>());
|
|
builder.Services.AddHostedService(sp => sp.GetRequiredService<DifferentialDrive>());
|
|
builder.Services.AddSingleton<OdometryService>();
|
|
builder.Services.AddHostedService(sp => sp.GetRequiredService<OdometryService>());
|
|
builder.Services.AddHostedService<OdometryBroadcastService>();
|
|
|
|
// Rotation Module services
|
|
// builder.Services.AddSingleton<RotationModuleService>();
|
|
// builder.Services.AddSingleton<IRotationModule>(sp => sp.GetRequiredService<RotationModuleService>());
|
|
// builder.Services.AddHostedService(sp => sp.GetRequiredService<RotationModuleService>());
|
|
|
|
// Lift Module: bật khi Modules:LiftModule:Enable = true
|
|
var liftModuleEnabled = builder.Configuration.GetValue<bool>("Modules:LiftModule:Enable");
|
|
if (liftModuleEnabled)
|
|
{
|
|
builder.Services.AddSingleton<LiftModuleService>();
|
|
builder.Services.AddSingleton<ILiftModule>(sp => sp.GetRequiredService<LiftModuleService>());
|
|
builder.Services.AddHostedService(sp => sp.GetRequiredService<LiftModuleService>());
|
|
}
|
|
else
|
|
{
|
|
builder.Services.AddSingleton<ILiftModule, NoOpLiftModule>();
|
|
}
|
|
|
|
// Rotation Module (vẫn dùng NoOp khi chưa bật)
|
|
builder.Services.AddSingleton<IRotationModule, NoOpRotationModule>();
|
|
|
|
builder.Services.AddSingleton<ManualControlService>();
|
|
builder.Services.AddHostedService(sp => sp.GetRequiredService<ManualControlService>());
|
|
builder.Services.AddSingleton<PS5ControllerService>();
|
|
builder.Services.AddHostedService(sp => sp.GetRequiredService<PS5ControllerService>());
|
|
|
|
// builder.Services.AddCartographer(builder.Configuration.GetSection("Cartographer"));
|
|
|
|
// builder.Services.AddSingleton<IMarkerDetector, MarkerDetector>();
|
|
|
|
// Navigation Monitor (Telemetry & Safety)
|
|
// builder.Services.AddSingleton<NavigationMonitorService>();
|
|
// builder.Services.AddHostedService(sp => sp.GetRequiredService<NavigationMonitorService>());
|
|
|
|
#if DEBUG
|
|
var dllPath = builder.Configuration["ScriptEngine:RuntimeDllFolder"] ?? "dlls";
|
|
if(!Directory.Exists(dllPath))
|
|
{
|
|
Directory.CreateDirectory(dllPath);
|
|
}
|
|
var netCoreDir = Path.GetDirectoryName(typeof(object).Assembly.Location);
|
|
if (!string.IsNullOrEmpty(netCoreDir))
|
|
{
|
|
var coreSrc = Path.Combine(netCoreDir, "System.Private.CoreLib.dll");
|
|
var runtimeSrc = Path.Combine(netCoreDir, "System.Runtime.dll");
|
|
var linqSrc = Path.Combine(netCoreDir, "System.Linq.Expressions.dll");
|
|
var scriptSrc = typeof(RobotNet10.Script.ILogger).Assembly.Location;
|
|
var robotAppSrc = typeof(RobotNet10.RobotApp.Script.IRobot).Assembly.Location;
|
|
|
|
var coreDest = Path.Combine(dllPath, "System.Private.CoreLib.dll");
|
|
var runtimeDest = Path.Combine(dllPath, "System.Runtime.dll");
|
|
var linqDest = Path.Combine(dllPath, "System.Linq.Expressions.dll");
|
|
var scriptDest = Path.Combine(dllPath, "RobotNet10.Script.dll");
|
|
var robotAppDest = Path.Combine(dllPath, "RobotNet10.RobotApp.Script.dll");
|
|
|
|
if(!File.Exists(coreDest)) File.Copy(coreSrc, coreDest);
|
|
if(!File.Exists(runtimeDest)) File.Copy(runtimeSrc, runtimeDest);
|
|
if(!File.Exists(linqDest)) File.Copy(linqSrc, linqDest);
|
|
|
|
File.Copy(scriptSrc, scriptDest, true);
|
|
File.Copy(robotAppSrc, robotAppDest, true);
|
|
}
|
|
#endif
|
|
|
|
var app = builder.Build();
|
|
|
|
await app.Services.SeedApplicationDbAsync();
|
|
await app.Services.SeedScriptEngineDbAsync();
|
|
await app.Services.SeedMapManagerAsync();
|
|
|
|
var serviceProbe = app.Services.GetService<IServiceProviderIsService>();
|
|
if (serviceProbe?.IsService(typeof(TuningDbContext)) == true)
|
|
{
|
|
await app.Services.SeedTuningDbAsync();
|
|
}
|
|
|
|
// Configure the HTTP request pipeline.
|
|
if (app.Environment.IsDevelopment())
|
|
{
|
|
app.UseWebAssemblyDebugging();
|
|
app.UseMigrationsEndPoint();
|
|
}
|
|
else
|
|
{
|
|
app.UseExceptionHandler("/Error", createScopeForErrors: true);
|
|
// The default HSTS value is 30 days. You may want to change this for production scenarios, see https://aka.ms/aspnetcore-hsts.
|
|
app.UseHsts();
|
|
}
|
|
app.UseStatusCodePagesWithReExecute("/not-found", createScopeForStatusCodePages: true);
|
|
app.UseHttpsRedirection();
|
|
|
|
app.UseAntiforgery();
|
|
app.MapControllers();
|
|
|
|
app.MapStaticAssets();
|
|
app.MapRazorComponents<App>()
|
|
.AddInteractiveServerRenderMode()
|
|
.AddInteractiveWebAssemblyRenderMode()
|
|
.AddAdditionalAssemblies(typeof(RobotNet10.RobotApp.Client._Imports).Assembly);
|
|
|
|
// Add additional endpoints required by the Identity /Account Razor components.
|
|
app.MapAdditionalIdentityEndpoints();
|
|
app.MapScriptEngineHubs();
|
|
app.MapHub<DeviceHub>("/hubs/devices");
|
|
app.MapHub<BatteryHub>("/hubs/devices/battery");
|
|
app.MapHub<InertialMeasurementUnitHub>("/hubs/devices/inertialmeasurementunit");
|
|
app.MapHub<LidarHub>("/hubs/devices/lidar");
|
|
app.MapHub<ModbusTcpHub>("/hubs/devices/modbustcp");
|
|
// app.MapHub<RfHandleHub>("/hubs/devices/rfhandle");
|
|
app.MapHub<CameraQrHub>("/hubs/devices/cameraqr");
|
|
app.MapHub<CiA402ServoHub>("/hubs/devices/cia402servo");
|
|
app.MapHub<MotionHub>("/hubs/motion");
|
|
app.MapHub<OdometryHub>("/hubs/odometry");
|
|
app.MapHub<XlocPoseHub>("/hubs/xloc/pose");
|
|
// app.MapHub<SLAMHub>("/hubs/slam");
|
|
// app.MapHub<TuningHub>("/tuninghub");
|
|
app.MapHub<PlcControllerHub>("/hubs/plc/controller");
|
|
// app.MapHub<MarkerDetectorHub>("hubs/marker-detect");
|
|
// app.MapHub<NavigationMonitorHub>("/hubs/motion/navigation-monitor");
|
|
|
|
var logger = app.Services.GetRequiredService<ILogger<Program>>();
|
|
app.MapGet("/api/motion/ps5/status", (PS5ControllerService svc) =>
|
|
{
|
|
return Results.Json(new { state = svc.State.ToString() });
|
|
});
|
|
|
|
app.MapPost("/api/motion/ps5/enable", async (PS5ControllerService svc) =>
|
|
{
|
|
svc.Enable();
|
|
return Results.Ok("PS5 controller enabled");
|
|
});
|
|
|
|
app.MapPost("/api/motion/ps5/disable", (PS5ControllerService svc) =>
|
|
{
|
|
svc.Disable();
|
|
return Results.Ok("PS5 controller disabled");
|
|
});
|
|
|
|
// PLC Lift module API (giao diện sửa: homing, velocity, position — 10000 = 0.01m)
|
|
var plcApi = app.MapGroup("/api/plc").DisableAntiforgery();
|
|
plcApi.MapPost("/lift/homing", (IPlcController plc) =>
|
|
{
|
|
plc.LiftHoming();
|
|
return Results.Ok(new { status = "ok", message = "Lift homing triggered" });
|
|
});
|
|
plcApi.MapPost("/lift/velocity", async (HttpRequest req, IPlcController plc) =>
|
|
{
|
|
var body = await req.ReadFromJsonAsync<LiftVelocityRequest>();
|
|
if (body == null) return Results.BadRequest(new { status = "error", message = "Body required: { up: bool, down: bool }" });
|
|
plc.SetLiftVelocity(body.Up, body.Down);
|
|
return Results.Ok(new { status = "ok", up = body.Up, down = body.Down });
|
|
});
|
|
plcApi.MapPost("/lift/position", async (HttpRequest req, IPlcController plc) =>
|
|
{
|
|
var body = await req.ReadFromJsonAsync<LiftPositionRequest>();
|
|
if (body == null) return Results.BadRequest(new { status = "error", message = "Body required: { position: int } (10000 = 0.01m)" });
|
|
plc.SetLiftPositionAndGo(body.Position);
|
|
return Results.Ok(new { status = "ok", position = body.Position });
|
|
});
|
|
|
|
// XLOC Control API
|
|
var xlocApi = app.MapGroup("/api/xloc").DisableAntiforgery();
|
|
|
|
// Mapping control
|
|
xlocApi.MapPost("/mapping/start", ([FromServices] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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));
|
|
|
|
// Helper: read origin (Map frame in World frame) from active map's YAML
|
|
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
|
|
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);
|
|
}
|
|
|
|
// Map upload endpoint (kept for backward compatibility)
|
|
xlocApi.MapPost("/map/upload", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Navigation.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" });
|
|
});
|
|
*/
|
|
|
|
// Register XLOC SignalR Hub
|
|
// app.MapHub<RobotNet10.RobotApp.Hubs.XlocHub>("/hubs/xloc");
|
|
|
|
// Navigation Control API
|
|
var navApi = app.MapGroup("/api/navigation").DisableAntiforgery();
|
|
|
|
// Navigation control - Move to goal
|
|
navApi.MapPost("/move_to", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service,
|
|
[FromServices] RobotNet10.RobotApp.Xloc.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] RobotNet10.RobotApp.Navigation.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] RobotNet10.RobotApp.Navigation.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()
|
|
});
|
|
});
|
|
|
|
// Marker Detection Control API
|
|
var markerApi = app.MapGroup("/api/marker-detection").DisableAntiforgery();
|
|
|
|
// Enable or disable marker detection
|
|
markerApi.MapPost("/detection/enable", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.MarkerDetection.MarkerDetectionIntegrationService service) => {
|
|
try
|
|
{
|
|
var body = await request.ReadFromJsonAsync<Dictionary<string, bool>>();
|
|
var enable = body?["enable"] ?? true;
|
|
|
|
service.SetEnableDetection(enable);
|
|
|
|
return Results.Ok(new {
|
|
status = "success",
|
|
message = $"Marker detection {(enable ? "enabled" : "disabled")}",
|
|
enabled = enable
|
|
});
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
return Results.BadRequest(new { status = "error", message = ex.Message });
|
|
}
|
|
});
|
|
|
|
// Get current marker pose
|
|
markerApi.MapGet("/pose/current", ([FromServices] RobotNet10.RobotApp.MarkerDetection.MarkerDetectionIntegrationService service) => {
|
|
try
|
|
{
|
|
var pose = service.GetMarkerPose();
|
|
|
|
if (pose == null)
|
|
return Results.NotFound(new { status = "error", message = "No marker pose available" });
|
|
|
|
return Results.Ok(new {
|
|
status = "success",
|
|
pose = new {
|
|
header = new {
|
|
seq = pose.Header.Seq,
|
|
stamp = pose.Header.Stamp,
|
|
frameId = pose.Header.FrameId
|
|
},
|
|
position = new {
|
|
x = pose.Pose.Position[0],
|
|
y = pose.Pose.Position[1],
|
|
z = pose.Pose.Position[2]
|
|
},
|
|
orientation = new {
|
|
x = pose.Pose.Orientation[0],
|
|
y = pose.Pose.Orientation[1],
|
|
z = pose.Pose.Orientation[2],
|
|
w = pose.Pose.Orientation[3]
|
|
}
|
|
}
|
|
});
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
return Results.BadRequest(new { status = "error", message = ex.Message });
|
|
}
|
|
});
|
|
|
|
app.Run();
|
|
|
|
// DTOs for PLC Lift API
|
|
public record LiftVelocityRequest(bool Up, bool Down);
|
|
public record LiftPositionRequest(int Position);
|