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