557 lines
20 KiB
Plaintext
557 lines
20 KiB
Plaintext
@page "/localization"
|
|
@rendermode InteractiveWebAssemblyNoPrerender
|
|
@implements IAsyncDisposable
|
|
|
|
@using MudBlazor
|
|
@using RobotNet10.RobotApp.Client.Clients
|
|
@using RobotNet10.RobotApp.Client.Components.SLAM
|
|
@using RobotNet10.RobotApp.Client.Dialogs
|
|
@using RobotNet10.RobotApp.Client.Shared.SLAM
|
|
@using RobotNet10.RobotApp.Shared.Enums
|
|
@using RobotNet10.Shared.Detection
|
|
@using RobotNet10.Shared.Geometry
|
|
|
|
<PageTitle>Localization</PageTitle>
|
|
|
|
<div class="h-100 w-100 d-flex flex-column overflow-hidden">
|
|
<!-- Control Bar -->
|
|
<div class="position-relative d-flex flex-row align-items-center gap-3 flex-wrap px-1" style="background-color: var(--mud-palette-background);">
|
|
<!-- Map Name -->
|
|
<div class="d-flex flex-row align-items-center gap-2" style="min-width: 300px;">
|
|
<MudTextField @bind-Value="MapName"
|
|
Label="Map Name" ShrinkLabel
|
|
Variant="Variant.Outlined"
|
|
Margin="Margin.Dense"
|
|
Disabled="@(CurrentState != SLAMState.Ready)"
|
|
Style="max-width: 300px;" />
|
|
</div>
|
|
|
|
<!-- Controls -->
|
|
<div class="d-flex flex-row align-items-center gap-2">
|
|
<MudButtonGroup Variant="Variant.Filled" Color="Color.Primary">
|
|
@if (IsLocalizationActive)
|
|
{
|
|
@* Fit View Button - always visible when map is displayed *@
|
|
<MudTooltip Text="Fit View">
|
|
<MudIconButton Color="Color.Default" Variant="Variant.Outlined"
|
|
Icon="@Icons.Material.Filled.FitScreen"
|
|
OnClick="FitViewAsync" />
|
|
</MudTooltip>
|
|
|
|
@* Localizing or InitializingLocalizing: Show Stop, Initial Pose, Marker Detect *@
|
|
<MudTooltip Text="Stop">
|
|
<MudIconButton Color="Color.Error" Variant="Variant.Filled"
|
|
Icon="@Icons.Material.Filled.Stop"
|
|
OnClick="StopLocalizationAsync" />
|
|
</MudTooltip>
|
|
<MudTooltip Text="Initialize Pose">
|
|
<MudIconButton Color="Color.Secondary" Variant="Variant.Filled"
|
|
Icon="@Icons.Material.Filled.MyLocation"
|
|
OnClick="SetInitialPoseAsync" />
|
|
</MudTooltip>
|
|
<MudTooltip Text="@(_isDetecting ? "Stop Detect" : "Marker Detect")">
|
|
<MudIconButton Color="@(_isDetecting? Color.Error: Color.Tertiary)" Variant="Variant.Filled"
|
|
Icon="@(_isDetecting ? Icons.Material.Filled.StopCircle : Icons.Material.Filled.Search)"
|
|
OnClick="ToggleMarkerDetectAsync"
|
|
Disabled="@(CurrentState == SLAMState.Relocalizing)" />
|
|
</MudTooltip>
|
|
}
|
|
else if (IsScanMappingActive)
|
|
{
|
|
@* Fit View Button - always visible when map is displayed *@
|
|
<MudTooltip Text="Fit View">
|
|
<MudIconButton Color="Color.Default" Variant="Variant.Outlined"
|
|
Icon="@Icons.Material.Filled.FitScreen"
|
|
OnClick="FitViewAsync" />
|
|
</MudTooltip>
|
|
|
|
@* ScanMapping or SavingMap: Show Save Map *@
|
|
<MudTooltip Text="@(CurrentState == SLAMState.SavingMap ? "Saving..." : "Save Map")">
|
|
<MudIconButton Color="Color.Success" Variant="Variant.Filled"
|
|
Icon="@Icons.Material.Filled.Save"
|
|
OnClick="SaveMapAsync"
|
|
Disabled="@(CurrentState == SLAMState.SavingMap)" />
|
|
</MudTooltip>
|
|
}
|
|
else
|
|
{
|
|
@* Ready or other states: Show Localize and Scan Mapping *@
|
|
<MudTooltip Text="Localize">
|
|
<MudIconButton Color="Color.Primary" Variant="Variant.Filled"
|
|
Icon="@Icons.Material.Filled.LocationOn"
|
|
OnClick="StartLocalizationAsync"
|
|
Disabled="@(CurrentState != SLAMState.Ready)" />
|
|
</MudTooltip>
|
|
<MudTooltip Text="Scan Mapping">
|
|
<MudIconButton Color="Color.Primary" Variant="Variant.Filled"
|
|
Icon="@Icons.Material.Filled.Map"
|
|
OnClick="StartScanMappingAsync"
|
|
Disabled="@(CurrentState != SLAMState.Ready || string.IsNullOrWhiteSpace(MapName))" />
|
|
</MudTooltip>
|
|
}
|
|
</MudButtonGroup>
|
|
</div>
|
|
|
|
@* Map Save Progress Indicator *@
|
|
@if (CurrentState == SLAMState.SavingMap && _saveProgress >= 0)
|
|
{
|
|
<div class="d-flex flex-row align-items-center gap-2" style="min-width: 300px;">
|
|
<MudProgressLinear Color="Color.Primary"
|
|
Value="@_saveProgress"
|
|
Class="flex-grow-1"
|
|
Striped="true"
|
|
Size="Size.Medium" />
|
|
<span class="small fw-medium">@_saveProgress%</span>
|
|
<span class="small">@(_workItemsCompleted)/@(_workItemsAdded)</span>
|
|
</div>
|
|
}
|
|
<div class="flex-grow-1"></div>
|
|
<!-- State Indicator -->
|
|
<div class="d-flex flex-row align-items-center gap-2">
|
|
<span class="status-chip d-inline-flex align-items-center px-3 py-1 rounded-pill small fw-medium" style="background-color: @GetStateChipBackgroundColor(CurrentState); color: @GetStateChipTextColor(CurrentState);">
|
|
@GetStateText(CurrentState ?? SLAMState.Idle)
|
|
</span>
|
|
</div>
|
|
<MudOverlay Visible="@(!CartographerClient.IsConnected)" AutoClose="false" DarkBackground Absolute />
|
|
</div>
|
|
|
|
<!-- Map Localization - Fill remaining space -->
|
|
<div class="flex-grow-1 w-100" style="min-height: 0;">
|
|
<MapLocalization @ref="MapLocalizationRef">
|
|
<Elements>
|
|
<MarkerDetectOverlay @ref="MarkerDetectOverlayRef" />
|
|
</Elements>
|
|
</MapLocalization>
|
|
</div>
|
|
</div>
|
|
|
|
<MudThemeProvider IsDarkMode />
|
|
<MudPopoverProvider />
|
|
<MudDialogProvider />
|
|
<MudSnackbarProvider />
|
|
|
|
@code {
|
|
private SLAMState? CurrentState { get; set; }
|
|
private bool IsLocalizationActive => CurrentState == SLAMState.Localizing || CurrentState == SLAMState.Relocalizing;
|
|
private bool IsScanMappingActive => CurrentState == SLAMState.ScanMapping || CurrentState == SLAMState.SavingMap;
|
|
private MapLocalization? MapLocalizationRef { get; set; }
|
|
private MarkerDetectOverlay? MarkerDetectOverlayRef { get; set; }
|
|
|
|
[Inject]
|
|
private SLAMClient CartographerClient { get; set; } = null!;
|
|
|
|
[Inject]
|
|
private ISnackbar Snackbar { get; set; } = null!;
|
|
|
|
[Inject]
|
|
private IDialogService DialogService { get; set; } = null!;
|
|
|
|
[Inject]
|
|
private MarkerDetectorHubClient MarkerDetectorClient { get; set; } = null!;
|
|
|
|
// Marker detection state
|
|
private Guid? _detectSessionId;
|
|
private MarkersSearchRequest? _detectRequest;
|
|
private System.Timers.Timer? _detectTimer;
|
|
private bool _isDetecting;
|
|
private string MapName = "";
|
|
|
|
// Map save progress state
|
|
private int _saveProgress = -1;
|
|
private int _workItemsAdded;
|
|
private int _workItemsCompleted;
|
|
|
|
protected override async Task OnAfterRenderAsync(bool firstRender)
|
|
{
|
|
if (firstRender)
|
|
{
|
|
await CartographerClient.StartAsync();
|
|
|
|
// Subscribe to events
|
|
CartographerClient.StateChanged += OnStateChanged;
|
|
CartographerClient.MapSaveProgressChanged += OnMapSaveProgressChanged;
|
|
|
|
// Get initial state
|
|
CurrentState = await CartographerClient.GetCurrentStateAsync();
|
|
|
|
// If currently in localization or scan mapping state, get the current map name
|
|
if (CurrentState == SLAMState.Relocalizing ||
|
|
CurrentState == SLAMState.Localizing ||
|
|
CurrentState == SLAMState.ScanMapping)
|
|
{
|
|
var currentMap = await CartographerClient.GetCurrentMapAsync();
|
|
if (!string.IsNullOrEmpty(currentMap))
|
|
{
|
|
MapName = currentMap;
|
|
}
|
|
}
|
|
|
|
StateHasChanged();
|
|
}
|
|
}
|
|
|
|
private void OnStateChanged(SLAMState state)
|
|
{
|
|
CurrentState = state;
|
|
|
|
// Reset progress when not saving
|
|
if (state != SLAMState.SavingMap)
|
|
{
|
|
_saveProgress = -1;
|
|
_workItemsAdded = 0;
|
|
_workItemsCompleted = 0;
|
|
}
|
|
|
|
StateHasChanged();
|
|
}
|
|
|
|
private void OnMapSaveProgressChanged(int workItemsAdded, int workItemsCompleted, int percentComplete)
|
|
{
|
|
_workItemsAdded = workItemsAdded;
|
|
_workItemsCompleted = workItemsCompleted;
|
|
_saveProgress = percentComplete;
|
|
StateHasChanged();
|
|
}
|
|
|
|
private async Task StartLocalizationAsync()
|
|
{
|
|
try
|
|
{
|
|
if (!CartographerClient.IsConnected)
|
|
{
|
|
Snackbar.Add("Not connected to server", Severity.Warning);
|
|
return;
|
|
}
|
|
|
|
// Get list of available maps
|
|
var maps = await CartographerClient.ListMapsAsync();
|
|
if (maps == null || maps.Length == 0)
|
|
{
|
|
Snackbar.Add("No maps available. Please create a map first.", Severity.Warning);
|
|
return;
|
|
}
|
|
|
|
// Show dialog to select map
|
|
var parameters = new DialogParameters<SelectMapDialog>
|
|
{
|
|
{ x => x.Maps, maps }
|
|
};
|
|
|
|
var options = new DialogOptions
|
|
{
|
|
CloseOnEscapeKey = true,
|
|
MaxWidth = MaxWidth.Small,
|
|
FullWidth = true
|
|
};
|
|
|
|
var dialog = await DialogService.ShowAsync<SelectMapDialog>("Select Map for Localization", parameters, options);
|
|
var result = await dialog.Result;
|
|
|
|
if (result != null && !result.Canceled && result.Data is string mapName)
|
|
{
|
|
// Start localization with selected map
|
|
var success = await CartographerClient.StartLocalizationAsync(mapName);
|
|
if (success)
|
|
{
|
|
MapName = mapName;
|
|
Snackbar.Add($"Started localization with map: {mapName}", Severity.Success);
|
|
StateHasChanged();
|
|
}
|
|
else
|
|
{
|
|
Snackbar.Add($"Failed to start localization with map: {mapName}", Severity.Error);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Snackbar.Add($"Failed to start localization: {ex.Message}", Severity.Error);
|
|
}
|
|
}
|
|
|
|
private async Task StopLocalizationAsync()
|
|
{
|
|
try
|
|
{
|
|
await CartographerClient.StopLocalizationAsync();
|
|
MapName = "";
|
|
StateHasChanged();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Snackbar.Add($"Failed to stop localization: {ex.Message}", Severity.Error);
|
|
}
|
|
}
|
|
|
|
private async Task StartScanMappingAsync()
|
|
{
|
|
try
|
|
{
|
|
if (!CartographerClient.IsConnected)
|
|
{
|
|
Snackbar.Add("Not connected to server", Severity.Warning);
|
|
return;
|
|
}
|
|
if (string.IsNullOrWhiteSpace(MapName))
|
|
{
|
|
Snackbar.Add("Please enter a map name.", Severity.Warning);
|
|
return;
|
|
}
|
|
var success = await CartographerClient.StartScanMappingAsync(MapName);
|
|
if (success)
|
|
Snackbar.Add("Scan mapping started", Severity.Success);
|
|
else
|
|
Snackbar.Add("Failed to start scan mapping", Severity.Error);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Snackbar.Add($"Failed to start scan mapping: {ex.Message}", Severity.Error);
|
|
}
|
|
}
|
|
|
|
private async Task SaveMapAsync()
|
|
{
|
|
try
|
|
{
|
|
_saveProgress = 0;
|
|
_workItemsAdded = 0;
|
|
_workItemsCompleted = 0;
|
|
StateHasChanged();
|
|
|
|
var mapPath = await CartographerClient.SaveMapAsync();
|
|
if (!string.IsNullOrEmpty(mapPath))
|
|
Snackbar.Add($"Map saved successfully: {mapPath}", Severity.Success);
|
|
else
|
|
Snackbar.Add("Map save initiated. Monitor progress above.", Severity.Info);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Snackbar.Add($"Failed to save map: {ex.Message}", Severity.Error);
|
|
}
|
|
}
|
|
|
|
private async Task SetInitialPoseAsync()
|
|
{
|
|
try
|
|
{
|
|
var pose = MapLocalizationRef?.GetGoalPoseDto();
|
|
if (pose == null)
|
|
{
|
|
Snackbar.Add("No goal pose set. Right-click on map to set robot goal, then click Initial Pose.", Severity.Warning);
|
|
return;
|
|
}
|
|
await CartographerClient.SetInitialPoseAsync(pose);
|
|
Snackbar.Add("Initial pose set from goal.", Severity.Success);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Snackbar.Add($"Failed to set initial pose: {ex.Message}", Severity.Error);
|
|
}
|
|
}
|
|
|
|
private async Task FitViewAsync()
|
|
{
|
|
if (MapLocalizationRef != null)
|
|
{
|
|
await MapLocalizationRef.FitViewAsync();
|
|
}
|
|
}
|
|
|
|
private async Task ToggleMarkerDetectAsync()
|
|
{
|
|
if (_isDetecting)
|
|
{
|
|
await StopMarkerDetectAsync();
|
|
}
|
|
else
|
|
{
|
|
await OpenMarkersSearchRequestDialogAsync();
|
|
}
|
|
}
|
|
|
|
private async Task OpenMarkersSearchRequestDialogAsync()
|
|
{
|
|
var options = new DialogOptions
|
|
{
|
|
CloseOnEscapeKey = true,
|
|
MaxWidth = MaxWidth.Medium,
|
|
FullWidth = true
|
|
};
|
|
|
|
var dialog = await DialogService.ShowAsync<MarkersSearchRequestDialog>("Create Markers Search Request", options);
|
|
var result = await dialog.Result;
|
|
|
|
if (result is not { Canceled: false, Data: MarkersSearchRequest request })
|
|
return;
|
|
|
|
try
|
|
{
|
|
// Stop previous detection if running
|
|
if (_isDetecting)
|
|
await StopMarkerDetectAsync();
|
|
|
|
// Connect and create session
|
|
await MarkerDetectorClient.StartAsync();
|
|
var createResult = await MarkerDetectorClient.CreateSessionAsync(request);
|
|
if (!createResult.IsSuccess)
|
|
{
|
|
Snackbar.Add($"Failed to create session: {createResult.Message}", Severity.Error);
|
|
return;
|
|
}
|
|
|
|
_detectSessionId = createResult.Data;
|
|
_detectRequest = request;
|
|
_isDetecting = true;
|
|
|
|
// Start polling for goal
|
|
_detectTimer = new System.Timers.Timer(1000);
|
|
_detectTimer.Elapsed += OnDetectTimerElapsed;
|
|
_detectTimer.Start();
|
|
|
|
Snackbar.Add($"Marker detection started (session: {_detectSessionId})", Severity.Success);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Snackbar.Add($"Failed to start marker detection: {ex.Message}", Severity.Error);
|
|
}
|
|
}
|
|
|
|
private async Task StopMarkerDetectAsync()
|
|
{
|
|
if (_detectTimer != null)
|
|
{
|
|
_detectTimer.Stop();
|
|
_detectTimer.Elapsed -= OnDetectTimerElapsed;
|
|
_detectTimer.Dispose();
|
|
_detectTimer = null;
|
|
}
|
|
|
|
_isDetecting = false;
|
|
_detectSessionId = null;
|
|
_detectRequest = null;
|
|
|
|
MarkerDetectOverlayRef?.Clear();
|
|
|
|
try
|
|
{
|
|
await MarkerDetectorClient.StopAsync();
|
|
}
|
|
catch { }
|
|
|
|
Snackbar.Add("Marker detection stopped.", Severity.Info);
|
|
}
|
|
|
|
private async void OnDetectTimerElapsed(object? sender, System.Timers.ElapsedEventArgs e)
|
|
{
|
|
if (!_detectSessionId.HasValue)
|
|
return;
|
|
|
|
try
|
|
{
|
|
var result = await MarkerDetectorClient.GetGoalAsync(_detectSessionId.Value);
|
|
Console.WriteLine($"goal = [{result.Data.Position.X}, {result.Data.Position.Y}, {result.Data.Position.Z}] [{result.Data.Orientation.X}, {result.Data.Orientation.Y}, {result.Data.Orientation.Z}, {result.Data.Orientation.W}]");
|
|
|
|
if (result.IsSuccess && result.Data is Pose goal && (goal.Orientation.W != 0 || goal.Orientation.Z != 0))
|
|
{
|
|
var points = ComputeReferencePointsWorld(goal);
|
|
await InvokeAsync(() => MarkerDetectOverlayRef?.Update(goal, points));
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"reject MarkerDetectorClient.GetGoalAsync");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine(ex);
|
|
}
|
|
}
|
|
|
|
private List<(double X, double Y)> ComputeReferencePointsWorld(Pose goal)
|
|
{
|
|
var points = new List<(double X, double Y)>();
|
|
if (_detectRequest is not MarkersSearchRequest req)
|
|
return points;
|
|
|
|
var yaw = goal.Orientation.ToYawRadian();
|
|
var cos = Math.Cos(yaw);
|
|
var sin = Math.Sin(yaw);
|
|
|
|
foreach (var entry in req.MarkerSearchRequests)
|
|
{
|
|
foreach (var pt in entry.ReferencePoints)
|
|
{
|
|
var wx = goal.Position.X + pt.X * cos - pt.Y * sin;
|
|
var wy = goal.Position.Y + pt.X * sin + pt.Y * cos;
|
|
points.Add((wx, wy));
|
|
}
|
|
}
|
|
return points;
|
|
}
|
|
|
|
private string GetStateChipBackgroundColor(SLAMState? state)
|
|
{
|
|
return state switch
|
|
{
|
|
SLAMState.Ready => "#4caf50",
|
|
SLAMState.Relocalizing => "#2196f3",
|
|
SLAMState.Localizing => "#2196f3",
|
|
SLAMState.ScanMapping => "#9c27b0",
|
|
SLAMState.SavingMap => "#ff9800",
|
|
SLAMState.Error => "#f44336",
|
|
_ => "#9e9e9e"
|
|
};
|
|
}
|
|
|
|
private string GetStateChipTextColor(SLAMState? state)
|
|
{
|
|
return "white";
|
|
}
|
|
|
|
private string GetStateText(SLAMState state)
|
|
{
|
|
return state switch
|
|
{
|
|
SLAMState.Idle => "Idle",
|
|
SLAMState.Initializing => "Initializing",
|
|
SLAMState.Ready => "Ready",
|
|
SLAMState.Relocalizing => "Relocalizing",
|
|
SLAMState.Localizing => "Localizing",
|
|
SLAMState.ScanMapping => "Scan Mapping",
|
|
SLAMState.SavingMap => "Saving Map",
|
|
SLAMState.Error => "Error",
|
|
_ => "Unknown"
|
|
};
|
|
}
|
|
|
|
public async ValueTask DisposeAsync()
|
|
{
|
|
// Unsubscribe from events
|
|
CartographerClient.StateChanged -= OnStateChanged;
|
|
CartographerClient.MapSaveProgressChanged -= OnMapSaveProgressChanged;
|
|
|
|
// Cleanup marker detection timer
|
|
if (_detectTimer != null)
|
|
{
|
|
_detectTimer.Stop();
|
|
_detectTimer.Elapsed -= OnDetectTimerElapsed;
|
|
_detectTimer.Dispose();
|
|
_detectTimer = null;
|
|
}
|
|
|
|
// Stop marker detector client
|
|
if (_isDetecting)
|
|
{
|
|
try
|
|
{
|
|
await MarkerDetectorClient.StopAsync();
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
_isDetecting = false;
|
|
_detectSessionId = null;
|
|
_detectRequest = null;
|
|
}
|
|
}
|