Initial commit
This commit is contained in:
@@ -0,0 +1,22 @@
|
||||
@using MudBlazor
|
||||
|
||||
<MudDialog>
|
||||
<TitleContent>
|
||||
<MudText Typo="Typo.h6">Delete Map</MudText>
|
||||
</TitleContent>
|
||||
<DialogContent>
|
||||
<MudText>Are you sure you want to delete map "@MapName"? This action cannot be undone.</MudText>
|
||||
</DialogContent>
|
||||
<DialogActions>
|
||||
<MudButton OnClick="Cancel">Cancel</MudButton>
|
||||
<MudButton Color="Color.Error" Variant="Variant.Filled" OnClick="Submit">Delete</MudButton>
|
||||
</DialogActions>
|
||||
</MudDialog>
|
||||
|
||||
@code {
|
||||
[CascadingParameter] IMudDialogInstance MudDialog { get; set; } = null!;
|
||||
[Parameter] public string MapName { get; set; } = string.Empty;
|
||||
|
||||
void Cancel() => MudDialog.Cancel();
|
||||
void Submit() => MudDialog.Close(DialogResult.Ok(true));
|
||||
}
|
||||
@@ -0,0 +1,121 @@
|
||||
@using Microsoft.AspNetCore.Components
|
||||
|
||||
<!-- Robot goal pose: line robot->goal, line goal->mouse (with arrow), circle at goal with radius = distance(goal,mouse).
|
||||
_goalOrientationRad được tính trong SetMousePosition: hướng từ goal đến mouse; 0 khi goal trùng mouse. -->
|
||||
<g visibility="@_visibility" @ref="_groupRef">
|
||||
<defs>
|
||||
<marker id="@_arrowMarkerId" markerWidth="0.15" markerHeight="0.15" refX="0" refY="0.075" orient="auto" markerUnits="userSpaceOnUse">
|
||||
<path d="M 0 0 L 0.15 0.075 L 0 0.15 Z" fill="#FF0000" stroke="none" />
|
||||
</marker>
|
||||
</defs>
|
||||
<!-- Line 1: RobotPosition -> GoalPosition (red, dashed, 0.1); zero length when robot==goal is valid -->
|
||||
<line x1="@_robotX" y1="@_robotY" x2="@_goalX" y2="@_goalY"
|
||||
stroke="green" stroke-width="0.03" stroke-dasharray="0.3,0.2" fill="none" />
|
||||
<!-- Line 2: GoalPosition -> current mouse (blue, dashed, 0.1, arrow at mouse) -->
|
||||
<line x1="@_goalX" y1="@_goalY" x2="@_mouseX" y2="@_mouseY"
|
||||
stroke="#FF0000" stroke-width="0.03" stroke-dasharray="0.3,0.2" fill="none"
|
||||
marker-end="url(#@_arrowMarkerId)" />
|
||||
<!-- Circle: center GoalPosition, radius = distance(Goal, mouse); r=0 when goal==mouse is valid -->
|
||||
<circle cx="@_goalX" cy="@_goalY" r="@_radius"
|
||||
stroke="green" stroke-width="0.03" stroke-dasharray="0.3,0.2" fill="none" />
|
||||
</g>
|
||||
|
||||
@code {
|
||||
private ElementReference _groupRef;
|
||||
private string _arrowMarkerId = "goal-mouse-arrow-" + Guid.NewGuid().ToString("N")[..8];
|
||||
|
||||
private double _robotX;
|
||||
private double _robotY;
|
||||
private double _goalX;
|
||||
private double _goalY;
|
||||
private double _goalOrientationRad;
|
||||
private double _mouseX;
|
||||
private double _mouseY;
|
||||
private string _visibility = "hidden";
|
||||
|
||||
/// <summary>Tọa độ X điểm đích (world coordinates).</summary>
|
||||
public double GoalX => _goalX;
|
||||
/// <summary>Tọa độ Y điểm đích (world coordinates).</summary>
|
||||
public double GoalY => _goalY;
|
||||
/// <summary>Hướng điểm đích (yaw, radians) — hướng từ goal đến mouse; 0 khi goal trùng mouse.</summary>
|
||||
public double GoalYaw => _goalOrientationRad;
|
||||
|
||||
/// <summary>Bán kính circle = khoảng cách Goal -> mouse; 0 khi goal trùng mouse (hợp lệ).</summary>
|
||||
private double _radius => Math.Sqrt((_mouseX - _goalX) * (_mouseX - _goalX) + (_mouseY - _goalY) * (_mouseY - _goalY));
|
||||
|
||||
/// <summary>
|
||||
/// Hiển thị nhóm line và circle (robot->goal, goal->mouse, circle).
|
||||
/// </summary>
|
||||
public void Show()
|
||||
{
|
||||
_visibility = "visible";
|
||||
StateHasChanged();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ẩn nhóm line và circle.
|
||||
/// </summary>
|
||||
public void Hide()
|
||||
{
|
||||
_visibility = "hidden";
|
||||
StateHasChanged();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Đặt tọa độ robot (world coordinates).
|
||||
/// </summary>
|
||||
public void SetRobotPosition(double x, double y)
|
||||
{
|
||||
_robotX = x;
|
||||
_robotY = y;
|
||||
StateHasChanged();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Đặt tọa độ điểm đích (world coordinates).
|
||||
/// </summary>
|
||||
public void SetGoalPosition(double x, double y)
|
||||
{
|
||||
_goalX = x;
|
||||
_goalY = y;
|
||||
StateHasChanged();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Cập nhật vị trí chuột (world coordinates). Parent gọi từ OnMouseMove để vẽ line goal->mouse và circle.
|
||||
/// Tự tính _goalOrientationRad = hướng từ goal đến mouse (radians); 0 khi goal trùng mouse.
|
||||
/// </summary>
|
||||
public void SetMousePosition(double x, double y)
|
||||
{
|
||||
_mouseX = x;
|
||||
_mouseY = y;
|
||||
var dx = _mouseX - _goalX;
|
||||
var dy = _mouseY - _goalY;
|
||||
_goalOrientationRad = (Math.Abs(dx) < 1e-9 && Math.Abs(dy) < 1e-9) ? 0 : Math.Atan2(dy, dx);
|
||||
StateHasChanged();
|
||||
}
|
||||
|
||||
private const double OptimizeMinDistanceMeters = 1.0;
|
||||
|
||||
/// <summary>
|
||||
/// Nếu khoảng cách từ (GoalX, GoalY) đến (mouseX, mouseY) < 1 m thì gọi Hide().
|
||||
/// Nếu khoảng cách >= 1 m thì đặt lại _mouseX, _mouseY sao cho khoảng cách đúng 1 m (giữ hướng).
|
||||
/// Tọa độ trong SVG là mét (viewBox + scaleY(-1) trong MapLocalization).
|
||||
/// </summary>
|
||||
public void Optimize()
|
||||
{
|
||||
var dx = _mouseX - _goalX;
|
||||
var dy = _mouseY - _goalY;
|
||||
var d = Math.Sqrt(dx * dx + dy * dy);
|
||||
if (d < OptimizeMinDistanceMeters)
|
||||
{
|
||||
Hide();
|
||||
return;
|
||||
}
|
||||
var scale = OptimizeMinDistanceMeters / d;
|
||||
_mouseX = _goalX + dx * scale;
|
||||
_mouseY = _goalY + dy * scale;
|
||||
_goalOrientationRad = (Math.Abs(dx) < 1e-9 && Math.Abs(dy) < 1e-9) ? 0 : Math.Atan2(dy, dx);
|
||||
StateHasChanged();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,404 @@
|
||||
@page "/map/{MapName}"
|
||||
@rendermode InteractiveWebAssemblyNoPrerender
|
||||
@implements IAsyncDisposable
|
||||
|
||||
@using MudBlazor
|
||||
@using RobotNet10.RobotApp.Client.Shared.SLAM
|
||||
@using Microsoft.JSInterop
|
||||
|
||||
@inject IJSRuntime JSRuntime
|
||||
@inject ISnackbar Snackbar
|
||||
|
||||
<PageTitle>Map: @MapName</PageTitle>
|
||||
|
||||
<MudThemeProvider IsDarkMode />
|
||||
<MudPopoverProvider />
|
||||
<MudDialogProvider />
|
||||
<MudSnackbarProvider />
|
||||
|
||||
<div class="map-edit-page">
|
||||
@* Control Bar *@
|
||||
<div class="control-bar">
|
||||
<MudStack Row="true" Spacing="2" AlignItems="AlignItems.Center">
|
||||
<MudTooltip Text="Back to Maps">
|
||||
<MudIconButton Icon="@Icons.Material.Filled.ArrowBack"
|
||||
Color="Color.Default"
|
||||
Size="Size.Medium" Href="/maps" />
|
||||
</MudTooltip>
|
||||
<MudDivider Vertical="true" FlexItem="true" Class="my-1" />
|
||||
<MudTooltip Text="Fit to View">
|
||||
<MudIconButton Icon="@Icons.Material.Filled.FitScreen"
|
||||
Color="Color.Primary"
|
||||
Size="Size.Medium"
|
||||
OnClick="FitViewAsync"
|
||||
Disabled="@(!IsMapLoaded)" />
|
||||
</MudTooltip>
|
||||
</MudStack>
|
||||
</div>
|
||||
|
||||
<div class="content-area">
|
||||
@* Processing Overlay *@
|
||||
<MudOverlay Visible="@MapInfo.IsProcessing" Absolute AutoClose="false" DarkBackground="true" ZIndex="1000">
|
||||
<MudStack AlignItems="AlignItems.Center" Spacing="2">
|
||||
<MudProgressCircular Indeterminate="true" Size="Size.Large" Color="Color.Primary" />
|
||||
<MudText Typo="Typo.h6" Color="Color.Primary">Processing map...</MudText>
|
||||
</MudStack>
|
||||
</MudOverlay>
|
||||
|
||||
@* Info Bar *@
|
||||
<div class="info-bar">
|
||||
<MudText Typo="Typo.h6" Class="mb-2">Map Information</MudText>
|
||||
<MudStack Spacing="1">
|
||||
<div class="info-item">
|
||||
<MudIcon Icon="@Icons.Material.Filled.Map" Size="Size.Small" Class="mr-1" />
|
||||
<MudText Typo="Typo.body2"><strong>Name:</strong> @MapInfo.Name</MudText>
|
||||
</div>
|
||||
<div class="info-item">
|
||||
<MudIcon Icon="@Icons.Material.Filled.CalendarToday" Size="Size.Small" Class="mr-1" />
|
||||
<MudText Typo="Typo.body2"><strong>Created:</strong> @MapInfo.CreatedDate.ToString("yyyy-MM-dd HH:mm")</MudText>
|
||||
</div>
|
||||
<div class="info-item">
|
||||
<MudIcon Icon="@Icons.Material.Filled.Straighten" Size="Size.Small" Class="mr-1" />
|
||||
<MudText Typo="Typo.body2"><strong>Resolution:</strong> @MapInfo.Resolution.ToString("F3") m/p</MudText>
|
||||
</div>
|
||||
<div class="info-item">
|
||||
<MudIcon Icon="@Icons.Material.Filled.AspectRatio" Size="Size.Small" Class="mr-1" />
|
||||
<MudText Typo="Typo.body2"><strong>Size:</strong> @MapInfo.Width.ToString("F1") x @MapInfo.Height.ToString("F1") m</MudText>
|
||||
</div>
|
||||
<div class="info-item">
|
||||
<MudIcon Icon="@Icons.Material.Filled.Timeline" Size="Size.Small" Class="mr-1" />
|
||||
<MudText Typo="Typo.body2"><strong>Nodes:</strong> @MapInfo.TrajectoryNodeCount</MudText>
|
||||
</div>
|
||||
<div class="info-item info-item-origin">
|
||||
<MudIcon Icon="@Icons.Material.Filled.MyLocation" Size="Size.Small" Class="mr-1" />
|
||||
<MudText Typo="Typo.body2"><strong>Origin:</strong> (@MapInfo.OriginX.ToString("F3"), @MapInfo.OriginY.ToString("F3"))</MudText>
|
||||
<MudSpacer />
|
||||
<MudTooltip Text="Edit Origin">
|
||||
<MudIconButton Icon="@Icons.Material.Filled.Edit"
|
||||
Size="Size.Small"
|
||||
Color="Color.Primary"
|
||||
OnClick="OpenEditOriginDialog"
|
||||
Disabled="@(!IsMapLoaded)" />
|
||||
</MudTooltip>
|
||||
</div>
|
||||
</MudStack>
|
||||
<MudDivider Class="my-2" />
|
||||
<MudButton Variant="Variant.Outlined"
|
||||
Color="Color.Primary"
|
||||
StartIcon="@Icons.Material.Filled.Refresh"
|
||||
OnClick="OpenRerenderDialog"
|
||||
Disabled="@(!IsMapLoaded || MapInfo.IsProcessing)"
|
||||
FullWidth="true"
|
||||
Size="Size.Small">
|
||||
Rerender Map
|
||||
</MudButton>
|
||||
<MudOverlay Visible="@IsLoading" Absolute AutoClose="false">
|
||||
<MudProgressCircular Indeterminate="true" Size="Size.Small" />
|
||||
</MudOverlay>
|
||||
</div>
|
||||
|
||||
@* Map View - structure similar to MapLocalization *@
|
||||
<div class="map-localization-container" @ref="containerRef" tabindex="1">
|
||||
<div @ref="viewMovementRef" class="map-view-movement">
|
||||
@* Map image with Y-flip (like map-canvas in MapLocalization) *@
|
||||
<img @ref="mapImageRef"
|
||||
src="@MapImageUrl"
|
||||
alt="Map"
|
||||
class="map-canvas" />
|
||||
@* SVG overlay for origin marker (like map-editor in MapLocalization) *@
|
||||
<svg @ref="mapContainerRef" class="map-editor" viewBox="0 0 0 0">
|
||||
<defs>
|
||||
<marker id="originvector" markerWidth="2.4" markerHeight="2.4" refX="0.1" refY="0.1">
|
||||
<line x1="0" y1="0.1" x2="0.5" y2="0.1" stroke="red" stroke-width="0.02" />
|
||||
<path d="M 0.5 0.15 L 0.6 0.1 L 0.5 0.05 Z" fill="red" stroke-width="0" />
|
||||
<line x1="0.1" y1="0" x2="0.1" y2="0.5" stroke="blue" stroke-width="0.02" />
|
||||
<path d="M 0.05 0.5 L 0.1 0.6 L 0.15 0.5 Z" fill="blue" stroke-width="0" />
|
||||
</marker>
|
||||
</defs>
|
||||
@* Robot goal pose component *@
|
||||
<GoalPose @ref="GoalPoseRef" />
|
||||
@* Grid origin marker *@
|
||||
<line class="origin" marker-end="url(#originvector)" />
|
||||
</svg>
|
||||
</div>
|
||||
<MapMousePosition @ref="MapMousePositionRef" />
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
@* Edit Origin Dialog *@
|
||||
<MudDialog @bind-Visible="_editOriginDialogVisible" Options="_editOriginDialogOptions">
|
||||
<TitleContent>
|
||||
<MudText Typo="Typo.h6">
|
||||
<MudIcon Icon="@Icons.Material.Filled.MyLocation" Class="mr-2" />
|
||||
Edit Map Origin
|
||||
</MudText>
|
||||
</TitleContent>
|
||||
<DialogContent>
|
||||
<MudText Typo="Typo.body2" Class="mb-3">
|
||||
Set the new origin position from the selected goal pose on the map.
|
||||
</MudText>
|
||||
<MudStack Spacing="2">
|
||||
<MudNumericField @bind-Value="_editOriginX" Label="X (meters)" Variant="Variant.Outlined" />
|
||||
<MudNumericField @bind-Value="_editOriginY" Label="Y (meters)" Variant="Variant.Outlined" />
|
||||
<MudNumericField @bind-Value="_editOriginYaw" Label="@(_editOriginYawUseRadian ? "Yaw (radians)" : "Yaw (degrees)")" Variant="Variant.Outlined"
|
||||
Adornment="Adornment.End" AdornmentIcon="@Icons.Material.Filled.CompareArrows"
|
||||
OnAdornmentClick="ToggleRadianDegree" AdornmentAriaLabel="Toggle radian/degree" />
|
||||
</MudStack>
|
||||
</DialogContent>
|
||||
<DialogActions>
|
||||
<MudButton OnClick="CloseEditOriginDialog">Cancel</MudButton>
|
||||
<MudButton Color="Color.Primary" Variant="Variant.Filled" OnClick="ConfirmEditOrigin">Confirm</MudButton>
|
||||
</DialogActions>
|
||||
</MudDialog>
|
||||
|
||||
@* Rerender Map Dialog *@
|
||||
<MudDialog @bind-Visible="_rerenderDialogVisible" Options="_rerenderDialogOptions">
|
||||
<TitleContent>
|
||||
<MudText Typo="Typo.h6">
|
||||
<MudIcon Icon="@Icons.Material.Filled.Refresh" Class="mr-2" />
|
||||
Rerender Map with Custom Config
|
||||
</MudText>
|
||||
</TitleContent>
|
||||
<DialogContent>
|
||||
<MudText Typo="Typo.body2" Class="mb-3">
|
||||
Configure occupancy grid settings and rerender map image files (PNG, JPG, PGM).
|
||||
</MudText>
|
||||
<MudStack Spacing="2">
|
||||
@* Merge Strategy *@
|
||||
<MudText Typo="Typo.subtitle2" Color="Color.Primary">Merge Strategy</MudText>
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudSelect T="SubmapMergeStrategyDto" @bind-Value="_rerenderConfig.MergeStrategy" Label="Merge Strategy" Variant="Variant.Outlined" Dense="true">
|
||||
<MudSelectItem Value="SubmapMergeStrategyDto.LogOddsSum">Log-Odds Sum (Bayesian)</MudSelectItem>
|
||||
<MudSelectItem Value="SubmapMergeStrategyDto.MaxProbability">Max Probability (Conservative)</MudSelectItem>
|
||||
<MudSelectItem Value="SubmapMergeStrategyDto.PorterDuff">Porter-Duff (Cairo-style)</MudSelectItem>
|
||||
</MudSelect>
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 300px;">
|
||||
<strong>Submap merge strategy:</strong><br/>
|
||||
• <b>Log-Odds Sum:</b> Sum Bayesian log-odds — clearer walls, less noise<br/>
|
||||
• <b>Max Probability:</b> Take max probability — safer for navigation, thicker walls<br/>
|
||||
• <b>Porter-Duff:</b> Cairo-style blending — matches the original C++ behavior; may blur in overlap areas
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudNumericField @bind-Value="_rerenderConfig.LogOddsClamp" Label="Log-Odds Clamp (1-20)" Variant="Variant.Outlined" Min="1.0" Max="20.0" Step="0.5" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Log-odds clamp:</strong><br/>
|
||||
• Low (1-5): smoother image, lower contrast<br/>
|
||||
• High (10-20): sharper walls, higher contrast<br/>
|
||||
• Default: 10 — balance between clarity and noise
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudCheckBox @bind-Value="_rerenderConfig.UseLogOddsAverage" Label="Use Log-Odds Average" Dense="true" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Use log-odds average:</strong><br/>
|
||||
• <b>On:</b> Divide log-odds by observation count — more uniform in overlap areas<br/>
|
||||
• <b>Off:</b> Sum directly — areas scanned more often become darker/lighter
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudDivider Class="my-1" />
|
||||
|
||||
@* Threshold Configuration *@
|
||||
<MudText Typo="Typo.subtitle2" Color="Color.Primary">Threshold Configuration</MudText>
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudNumericField @bind-Value="_rerenderConfig.FreeSpaceThreshold" Label="Free Space Threshold (0-255)" Variant="Variant.Outlined" Min="0" Max="255" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Free-space threshold:</strong><br/>
|
||||
Pixels with texture value >= this threshold are marked as FREE (white).<br/>
|
||||
• Low (50-80): more areas become free<br/>
|
||||
• High (150-200): only very certain areas become free<br/>
|
||||
• Default: 100
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudNumericField @bind-Value="_rerenderConfig.OccupiedSpaceThreshold" Label="Occupied Space Threshold (0-255)" Variant="Variant.Outlined" Min="0" Max="255" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Occupied-space threshold:</strong><br/>
|
||||
Pixels with alpha value > this threshold are marked as OCCUPIED (black).<br/>
|
||||
• Low (1-10): thicker walls, more sensitive to obstacles<br/>
|
||||
• High (50-100): only strong walls are drawn<br/>
|
||||
• Default: 1 (most sensitive)
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudDivider Class="my-1" />
|
||||
|
||||
@* Output Mode *@
|
||||
<MudText Typo="Typo.subtitle2" Color="Color.Primary">Output Mode</MudText>
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudCheckBox @bind-Value="_rerenderConfig.UseBinaryOutput" Label="Binary Output (0/100/-1)" Dense="true" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Binary output mode:</strong><br/>
|
||||
• <b>On:</b> Only 3 values: 0 (white/free), 100 (black/wall), -1 (gray/unknown). Suitable for MCL/Navigation.<br/>
|
||||
• <b>Off:</b> Gradient 0-100 values. Shows detailed occupancy probability.
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudDivider Class="my-1" />
|
||||
|
||||
@* Wall Thinning *@
|
||||
<MudText Typo="Typo.subtitle2" Color="Color.Primary">Wall Thinning (Post-processing)</MudText>
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudCheckBox @bind-Value="_rerenderConfig.EnableWallThinning" Label="Enable Wall Thinning" Dense="true" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Wall thinning:</strong><br/>
|
||||
Applies morphological erosion to reduce wall thickness.<br/>
|
||||
• <b>On:</b> Thinner walls; the robot can pass narrow corridors more easily<br/>
|
||||
• <b>Off:</b> Keep original wall thickness
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudNumericField @bind-Value="_rerenderConfig.WallThinningIterations" Label="Thinning Iterations (1-5)" Variant="Variant.Outlined" Min="1" Max="5" Disabled="@(!_rerenderConfig.EnableWallThinning)" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Thinning iterations:</strong><br/>
|
||||
Each iteration erodes ~1 pixel from the wall boundary.<br/>
|
||||
• 1 iteration: slightly thinner (~1 pixel)<br/>
|
||||
• 3-5 iterations: significantly thinner; walls may break/disconnect
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudNumericField @bind-Value="_rerenderConfig.MinWallThicknessPixels" Label="Min Wall Thickness (pixels, 1-10)" Variant="Variant.Outlined" Min="1" Max="10" Disabled="@(!_rerenderConfig.EnableWallThinning)" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Minimum wall thickness:</strong><br/>
|
||||
Do not erode if the wall is thinner than this value.<br/>
|
||||
• 1 pixel: allows very thin walls (may break)<br/>
|
||||
• 2-3 pixels: safer, preserves wall structure<br/>
|
||||
• With 0.05 m resolution: 2 pixels ≈ 10 cm real wall thickness
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudDivider Class="my-1" />
|
||||
|
||||
@* Ambiguous Cell Handling *@
|
||||
<MudText Typo="Typo.subtitle2" Color="Color.Primary">Ambiguous Cell Handling</MudText>
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudSelect T="sbyte" @bind-Value="_rerenderConfig.AmbiguousCellValue" Label="Ambiguous Cell Value" Variant="Variant.Outlined" Dense="true">
|
||||
<MudSelectItem Value="@((sbyte)-1)">Unknown (-1)</MudSelectItem>
|
||||
<MudSelectItem Value="@((sbyte)0)">Free (0)</MudSelectItem>
|
||||
<MudSelectItem Value="@((sbyte)100)">Occupied (100)</MudSelectItem>
|
||||
</MudSelect>
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Ambiguous cell value:</strong><br/>
|
||||
Cells with probability in the ambiguous range are assigned this value.<br/>
|
||||
• <b>Unknown (-1):</b> Gray; ignored by MCL — safest<br/>
|
||||
• <b>Free (0):</b> White; robot may pass — riskier<br/>
|
||||
• <b>Occupied (100):</b> Black; robot avoids — conservative
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudNumericField @bind-Value="_rerenderConfig.AmbiguousRangeLower" Label="Ambiguous Range Lower (0-1)" Variant="Variant.Outlined" Min="0.0" Max="1.0" Step="0.05" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Ambiguous range lower:</strong><br/>
|
||||
Probabilities below this value are considered FREE.<br/>
|
||||
• 0.35 (default): 0–35% is free<br/>
|
||||
• Lower to 0.2: stricter, fewer free areas<br/>
|
||||
• Raise to 0.45: more areas become free
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudNumericField @bind-Value="_rerenderConfig.AmbiguousRangeUpper" Label="Ambiguous Range Upper (0-1)" Variant="Variant.Outlined" Min="0.0" Max="1.0" Step="0.05" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Ambiguous range upper:</strong><br/>
|
||||
Probabilities above this value are considered OCCUPIED.<br/>
|
||||
• 0.65 (default): 65–100% is wall<br/>
|
||||
• Lower to 0.55: more walls (safer)<br/>
|
||||
• Raise to 0.8: only very certain areas become walls
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudDivider Class="my-1" />
|
||||
|
||||
@* Advanced Options *@
|
||||
<MudText Typo="Typo.subtitle2" Color="Color.Primary">Advanced Options</MudText>
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudCheckBox @bind-Value="_rerenderConfig.EnableMedianFilter" Label="Enable Median Filter" Dense="true" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Median filter:</strong><br/>
|
||||
Reduces salt-and-pepper noise.<br/>
|
||||
• <b>On:</b> Smoother image, removes isolated noisy pixels<br/>
|
||||
• <b>Off:</b> Keeps original details, may contain noise
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
|
||||
<MudTooltip Placement="Placement.Right" Arrow="true">
|
||||
<ChildContent>
|
||||
<MudNumericField @bind-Value="_rerenderConfig.MedianFilterKernelSize" Label="Median Filter Kernel Size (3,5,7)" Variant="Variant.Outlined" Min="3" Max="7" Step="2" Disabled="@(!_rerenderConfig.EnableMedianFilter)" />
|
||||
</ChildContent>
|
||||
<TooltipContent>
|
||||
<MudText Typo="Typo.body2" Style="max-width: 280px;">
|
||||
<strong>Filter kernel size:</strong><br/>
|
||||
Neighborhood used to compute the median.<br/>
|
||||
• 3x3: light filtering, preserves details<br/>
|
||||
• 5x5: medium filtering<br/>
|
||||
• 7x7: strong filtering, may blur wall edges
|
||||
</MudText>
|
||||
</TooltipContent>
|
||||
</MudTooltip>
|
||||
</MudStack>
|
||||
</DialogContent>
|
||||
<DialogActions>
|
||||
<MudButton OnClick="CloseRerenderDialog">Cancel</MudButton>
|
||||
<MudButton Color="Color.Primary" Variant="Variant.Filled" OnClick="ConfirmRerender">Rerender</MudButton>
|
||||
</DialogActions>
|
||||
</MudDialog>
|
||||
@@ -0,0 +1,604 @@
|
||||
using Microsoft.AspNetCore.Components;
|
||||
using Microsoft.JSInterop;
|
||||
using MudBlazor;
|
||||
using RobotNet10.RobotApp.Client.Clients;
|
||||
using RobotNet10.RobotApp.Client.Shared.SLAM;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using QuaternionGeometry = RobotNet10.Shared.Geometry.Quaternion;
|
||||
using QuaternionNumbers = RobotNet10.Shared.Numbers.Quaternion;
|
||||
|
||||
namespace RobotNet10.RobotApp.Client.Components.SLAM;
|
||||
|
||||
public partial class MapEdit
|
||||
{
|
||||
private const double MinFitScale = 0.5;
|
||||
|
||||
[Parameter]
|
||||
public string MapName { get; set; } = string.Empty;
|
||||
|
||||
[Inject]
|
||||
private SLAMClient SLAMClient { get; set; } = null!;
|
||||
|
||||
private IJSObjectReference _jsModule = null!;
|
||||
private DotNetObjectReference<MapEdit> _dotNetObj = null!;
|
||||
|
||||
// Element references
|
||||
private ElementReference containerRef;
|
||||
private ElementReference viewMovementRef;
|
||||
private ElementReference mapContainerRef;
|
||||
private ElementReference mapImageRef;
|
||||
private MapMousePosition MapMousePositionRef = null!;
|
||||
private GoalPose GoalPoseRef = null!;
|
||||
|
||||
// State
|
||||
private MapInfoDto MapInfo { get; set; } = new();
|
||||
private bool IsLoading { get; set; } = true;
|
||||
private bool IsMapLoaded => MapInfo != null && _imageLoaded;
|
||||
private bool _imageLoaded = false;
|
||||
|
||||
// Map image URL with cache busting
|
||||
private long _imageCacheBuster = DateTime.Now.Ticks;
|
||||
private string MapImageUrl => $"/api/maps/{MapName}/image?v={_imageCacheBuster}";
|
||||
|
||||
// Container rect state
|
||||
private double _containerRectX = 0.0;
|
||||
private double _containerRectY = 0.0;
|
||||
private double _containerRectWidth = 0.0;
|
||||
private double _containerRectHeight = 0.0;
|
||||
private double _containerRectTop = 0.0;
|
||||
private double _containerRectRight = 0.0;
|
||||
private double _containerRectBottom = 0.0;
|
||||
private double _containerRectLeft = 0.0;
|
||||
|
||||
// View state
|
||||
public double CursorX { get; private set; } = 0.0;
|
||||
public double CursorY { get; private set; } = 0.0;
|
||||
private double _clientOriginX = 0.0;
|
||||
private double _clientOriginY = 0.0;
|
||||
private double _scale = 1.0;
|
||||
private double _left = 0.0;
|
||||
private double _top = 0.0;
|
||||
private double _fitScale = 1.0;
|
||||
|
||||
// Map data
|
||||
private double _resolution = 1.0;
|
||||
private double _originX = 0.0;
|
||||
private double _originY = 0.0; // Transformed origin Y (like MapLocalization)
|
||||
private double _mapOriginY = 0.0; // Original origin Y from map (like MapLocalization._mapOriginY)
|
||||
private double _imageWidth = 0.0;
|
||||
private double _imageHeight = 0.0;
|
||||
private int _imagePixelWidth = 0;
|
||||
private int _imagePixelHeight = 0;
|
||||
|
||||
// SVG viewBox origin (like MapLocalization._svgOriginX and _svgOriginY)
|
||||
private double _svgOriginX = 0.0;
|
||||
private double _svgOriginY = 0.0;
|
||||
|
||||
#region Lifecycle
|
||||
|
||||
protected override async Task OnAfterRenderAsync(bool firstRender)
|
||||
{
|
||||
await base.OnAfterRenderAsync(firstRender);
|
||||
if (!firstRender) return;
|
||||
|
||||
// Load JavaScript module
|
||||
_jsModule = await JSRuntime.InvokeAsync<IJSObjectReference>("import", "/js/mapLocalization.js");
|
||||
|
||||
// Setup event listeners
|
||||
_dotNetObj = DotNetObjectReference.Create(this);
|
||||
await _jsModule.InvokeVoidAsync("updateContainerRect", _dotNetObj, containerRef, nameof(OnContainerResize));
|
||||
await _jsModule.InvokeVoidAsync("registerResizeObserver", _dotNetObj, containerRef, nameof(OnContainerResize));
|
||||
await _jsModule.InvokeVoidAsync("addMouseWheelEventListener", _dotNetObj, containerRef, nameof(OnMouseWheel));
|
||||
await _jsModule.InvokeVoidAsync("addMouseMoveEventListener", _dotNetObj, containerRef, nameof(OnMouseMove));
|
||||
await _jsModule.InvokeVoidAsync("addMouseDownEventListener", _dotNetObj, containerRef, nameof(OnMouseDown));
|
||||
await _jsModule.InvokeVoidAsync("addMouseUpEventListener", _dotNetObj, containerRef, nameof(OnMouseUp));
|
||||
|
||||
// Start SLAMClient and register event handler
|
||||
await SLAMClient.StartAsync();
|
||||
SLAMClient.IsProcessingChanged += OnIsProcessingChanged;
|
||||
|
||||
// Load map info
|
||||
await LoadMapInfoAsync();
|
||||
await OnImageLoaded();
|
||||
}
|
||||
|
||||
private async Task LoadMapInfoAsync()
|
||||
{
|
||||
try
|
||||
{
|
||||
IsLoading = true;
|
||||
StateHasChanged();
|
||||
|
||||
MapInfo = await SLAMClient.GetMapInfoAndSubscribeProcessingAsync(MapName) ?? new();
|
||||
if (!string.IsNullOrEmpty(MapInfo.Name))
|
||||
{
|
||||
_resolution = MapInfo.Resolution;
|
||||
_imageWidth = MapInfo.Width;
|
||||
_imageHeight = MapInfo.Height;
|
||||
_originX = MapInfo.OriginX;
|
||||
_mapOriginY = MapInfo.OriginY; // Original origin Y from map
|
||||
// Transformed origin Y (like MapLocalization: _originY = -imageHeight - mapOriginY)
|
||||
_originY = -_imageHeight - _mapOriginY;
|
||||
|
||||
// Calculate pixel dimensions from world dimensions and resolution
|
||||
if (_resolution > 0)
|
||||
{
|
||||
_imagePixelWidth = (int)Math.Round(_imageWidth / _resolution);
|
||||
_imagePixelHeight = (int)Math.Round(_imageHeight / _resolution);
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Snackbar.Add($"Failed to load map info: {ex.Message}", MudBlazor.Severity.Error);
|
||||
}
|
||||
finally
|
||||
{
|
||||
IsLoading = false;
|
||||
StateHasChanged();
|
||||
}
|
||||
}
|
||||
|
||||
private void OnIsProcessingChanged(string mapName, bool isProcessing)
|
||||
{
|
||||
if (mapName == MapName)
|
||||
{
|
||||
var wasProcessing = MapInfo.IsProcessing;
|
||||
MapInfo.IsProcessing = isProcessing;
|
||||
|
||||
// When processing completes (true -> false), reload map info and update image
|
||||
if (wasProcessing && !isProcessing)
|
||||
{
|
||||
InvokeAsync(async () =>
|
||||
{
|
||||
// Update cache buster to force image reload
|
||||
_imageCacheBuster = DateTime.Now.Ticks;
|
||||
|
||||
// Reload map info to get updated data
|
||||
await LoadMapInfoAsync();
|
||||
|
||||
// Force reload image with reset CSS styles and get natural dimensions
|
||||
await ReloadImageAsync();
|
||||
|
||||
GoalPoseRef.Hide();
|
||||
});
|
||||
}
|
||||
else
|
||||
{
|
||||
InvokeAsync(StateHasChanged);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private async Task ReloadImageAsync()
|
||||
{
|
||||
try
|
||||
{
|
||||
// Pixel dimensions already calculated in LoadMapInfoAsync from MapInfo
|
||||
_imageLoaded = true;
|
||||
StateHasChanged();
|
||||
|
||||
// Wait for DOM update and image reload
|
||||
await Task.Delay(100);
|
||||
|
||||
// Configure SVG viewBox with world coordinates first
|
||||
await ConfigureSvgViewBoxAsync();
|
||||
|
||||
// Then fit to view (scales SVG to pixel coords)
|
||||
await FitViewAsync();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.Error.WriteLine($"[MapEdit] ReloadImageAsync: Exception - {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
private async Task OnImageLoaded()
|
||||
{
|
||||
try
|
||||
{
|
||||
// Pixel dimensions already calculated in LoadMapInfoAsync from MapInfo
|
||||
_imageLoaded = true;
|
||||
StateHasChanged();
|
||||
|
||||
// Wait for DOM update and image load
|
||||
await Task.Delay(100);
|
||||
|
||||
// Configure SVG viewBox with world coordinates first
|
||||
await ConfigureSvgViewBoxAsync();
|
||||
|
||||
// Then fit to view (scales SVG to pixel coords)
|
||||
await FitViewAsync();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.Error.WriteLine($"[MapEdit] OnImageLoaded: Exception - {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region View & Scale
|
||||
|
||||
public async Task FitViewAsync()
|
||||
{
|
||||
if (!IsMapLoaded || _imageWidth <= 0 || _imageHeight <= 0) return;
|
||||
|
||||
// Recalculate fit scale
|
||||
if (_containerRectWidth > 0 && _containerRectHeight > 0)
|
||||
{
|
||||
_fitScale = Math.Min(_containerRectWidth / _imageWidth, _containerRectHeight / _imageHeight);
|
||||
if (_fitScale < MinFitScale)
|
||||
_fitScale = MinFitScale;
|
||||
}
|
||||
|
||||
await ScaleFitContentAsync();
|
||||
}
|
||||
|
||||
private void UpdateClientOrigin()
|
||||
{
|
||||
if (!IsMapLoaded) return;
|
||||
|
||||
_clientOriginX = _containerRectLeft + _left - _originX * _scale;
|
||||
_clientOriginY = _containerRectTop + _top - _originY * _scale;
|
||||
}
|
||||
|
||||
private async Task ScaleFitContentAsync()
|
||||
{
|
||||
if (!IsMapLoaded) return;
|
||||
|
||||
_scale = _fitScale;
|
||||
|
||||
var wrapperWidth = _imageWidth * _scale;
|
||||
var wrapperHeight = _imageHeight * _scale;
|
||||
|
||||
var centerLeft = (_containerRectWidth - wrapperWidth) / 2;
|
||||
var centerTop = (_containerRectHeight - wrapperHeight) / 2;
|
||||
|
||||
await SetViewMovement(centerLeft, centerTop);
|
||||
|
||||
// Set SVG rect (pixel coords) and image sizes (like MapLocalization.ScaleFitContentAsync)
|
||||
await _jsModule.InvokeVoidAsync("setSvgRect", mapContainerRef, wrapperWidth, wrapperHeight);
|
||||
await _jsModule.InvokeVoidAsync("setCanvasRect", mapImageRef, wrapperWidth, wrapperHeight);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Configure SVG viewBox with world coordinates (like MapLocalization.DrawOccupancyGridAsync)
|
||||
/// </summary>
|
||||
private async Task ConfigureSvgViewBoxAsync()
|
||||
{
|
||||
if (!IsMapLoaded || _jsModule == null) return;
|
||||
|
||||
// Update SVG viewBox origin
|
||||
_svgOriginX = _originX;
|
||||
_svgOriginY = _mapOriginY;
|
||||
|
||||
// Set SVG config with world coordinates for viewBox
|
||||
await _jsModule.InvokeVoidAsync("setSvgConfig", mapContainerRef, _imageWidth, _imageHeight, _svgOriginX, _svgOriginY);
|
||||
}
|
||||
|
||||
private async Task SetViewMovement(double left, double top)
|
||||
{
|
||||
_top = top;
|
||||
_left = left;
|
||||
|
||||
UpdateClientOrigin();
|
||||
|
||||
if (_jsModule != null && IsMapLoaded)
|
||||
{
|
||||
var width = _imageWidth * _scale;
|
||||
var height = _imageHeight * _scale;
|
||||
await _jsModule.InvokeVoidAsync("setMapMovement", viewMovementRef, _top, _left, width, height);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Event Handlers (JSInvokable)
|
||||
|
||||
[JSInvokable]
|
||||
public void OnContainerResize(double x, double y, double width, double height, double top, double right, double bottom, double left)
|
||||
{
|
||||
_containerRectX = x;
|
||||
_containerRectY = y;
|
||||
_containerRectWidth = width;
|
||||
_containerRectHeight = height;
|
||||
_containerRectTop = top;
|
||||
_containerRectRight = right;
|
||||
_containerRectBottom = bottom;
|
||||
_containerRectLeft = left;
|
||||
|
||||
UpdateClientOrigin();
|
||||
|
||||
// Recalculate fit scale
|
||||
if (_imageWidth > 0 && _imageHeight > 0)
|
||||
{
|
||||
_fitScale = Math.Min(_containerRectWidth / _imageWidth, _containerRectHeight / _imageHeight);
|
||||
}
|
||||
}
|
||||
|
||||
[JSInvokable]
|
||||
public async Task OnMouseWheel(double deltaY, double clientX, double clientY)
|
||||
{
|
||||
if (!IsMapLoaded) return;
|
||||
|
||||
// Calculate scale change
|
||||
double scaleChange;
|
||||
if (deltaY > 0)
|
||||
{
|
||||
if (_scale <= _fitScale / 2) return;
|
||||
scaleChange = _scale > _fitScale ? -(_scale / _fitScale) : -0.1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (_scale >= _fitScale * 100) return;
|
||||
scaleChange = _scale < _fitScale ? 0.5 : (_scale / _fitScale);
|
||||
}
|
||||
|
||||
double oldScale = _scale;
|
||||
_scale += scaleChange;
|
||||
|
||||
// Set SVG rect (pixel coords) and image sizes (like MapLocalization)
|
||||
await _jsModule.InvokeVoidAsync("setSvgRect", mapContainerRef, _imageWidth * _scale, _imageHeight * _scale);
|
||||
await _jsModule.InvokeVoidAsync("setCanvasRect", mapImageRef, _imageWidth * _scale, _imageHeight * _scale);
|
||||
|
||||
// Calculate cursor position in world coordinates (exactly like MapLocalization)
|
||||
CursorX = (clientX - _clientOriginX) / oldScale;
|
||||
CursorY = (_clientOriginY - clientY) / oldScale;
|
||||
MapMousePositionRef?.Update(CursorX, CursorY);
|
||||
|
||||
// Calculate mouse position relative to map origin (exactly like MapLocalization)
|
||||
// MapLocalization: mouseX = CursorX - OriginX
|
||||
// MapLocalization: mouseY = CursorY - MapData.OriginY (use original origin Y, not transformed)
|
||||
double mouseX = CursorX - _originX;
|
||||
double mouseY = CursorY - _mapOriginY;
|
||||
|
||||
// Calculate movement adjustment (exactly like MapLocalization)
|
||||
await SetViewMovement(_left - mouseX * scaleChange, _top - (_imageHeight - mouseY) * scaleChange);
|
||||
}
|
||||
|
||||
[JSInvokable]
|
||||
public async Task OnMouseMove(double clientX, double clientY, long buttons, bool ctrlKey, double movementX, double movementY)
|
||||
{
|
||||
// Calculate cursor position in world coordinates
|
||||
CursorX = (clientX - _clientOriginX) / _scale;
|
||||
CursorY = (_clientOriginY - clientY) / _scale;
|
||||
MapMousePositionRef?.Update(CursorX, CursorY);
|
||||
|
||||
// Left mouse button down: update goal pose to current mouse
|
||||
if (buttons == 1)
|
||||
{
|
||||
GoalPoseRef?.SetMousePosition(CursorX, CursorY);
|
||||
}
|
||||
// Middle mouse button for panning
|
||||
else if (buttons == 4)
|
||||
{
|
||||
await SetViewMovement(_left + movementX, _top + movementY);
|
||||
}
|
||||
}
|
||||
|
||||
[JSInvokable]
|
||||
public async Task OnMouseDown(int button, bool altKey, bool ctrlKey, bool shiftKey)
|
||||
{
|
||||
if (button == 0) // Left mouse button: set goal pose
|
||||
{
|
||||
// For MapEdit, robot position is at origin (0, 0) since we don't have live robot pose
|
||||
GoalPoseRef?.SetRobotPosition(0, 0);
|
||||
GoalPoseRef?.SetGoalPosition(CursorX, CursorY);
|
||||
GoalPoseRef?.SetMousePosition(CursorX, CursorY);
|
||||
GoalPoseRef?.Show();
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
[JSInvokable]
|
||||
public async Task OnMouseUp(int button, bool altKey, bool ctrlKey, bool shiftKey)
|
||||
{
|
||||
if (button == 0) // Left mouse button up: apply Optimize
|
||||
{
|
||||
GoalPoseRef?.Optimize();
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Edit Origin Dialog
|
||||
|
||||
// Dialog state
|
||||
private bool _editOriginDialogVisible = false;
|
||||
private readonly DialogOptions _editOriginDialogOptions = new()
|
||||
{
|
||||
CloseOnEscapeKey = false,
|
||||
CloseButton = true,
|
||||
BackdropClick = false,
|
||||
MaxWidth = MaxWidth.Small,
|
||||
FullWidth = true
|
||||
};
|
||||
|
||||
// Dialog form values
|
||||
private double _editOriginX = 0.0;
|
||||
private double _editOriginY = 0.0;
|
||||
private double _editOriginYaw = 0.0;
|
||||
private bool _editOriginYawUseRadian = true;
|
||||
|
||||
private void OpenEditOriginDialog()
|
||||
{
|
||||
// Reset to radians when opening dialog (GoalYaw is in radians)
|
||||
_editOriginYawUseRadian = true;
|
||||
|
||||
if (GoalPoseRef == null)
|
||||
{
|
||||
_editOriginX = MapInfo.OriginX;
|
||||
_editOriginY = MapInfo.OriginY;
|
||||
_editOriginYaw = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Get values from GoalPoseRef
|
||||
_editOriginX = GoalPoseRef.GoalX;
|
||||
_editOriginY = GoalPoseRef.GoalY;
|
||||
_editOriginYaw = GoalPoseRef.GoalYaw;
|
||||
}
|
||||
|
||||
_editOriginDialogVisible = true;
|
||||
}
|
||||
|
||||
private void CloseEditOriginDialog()
|
||||
{
|
||||
_editOriginDialogVisible = false;
|
||||
}
|
||||
|
||||
private void ToggleRadianDegree()
|
||||
{
|
||||
if (_editOriginYawUseRadian)
|
||||
{
|
||||
// Convert from radians to degrees
|
||||
_editOriginYaw = _editOriginYaw * 180.0 / Math.PI;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Convert from degrees to radians
|
||||
_editOriginYaw = _editOriginYaw * Math.PI / 180.0;
|
||||
}
|
||||
_editOriginYawUseRadian = !_editOriginYawUseRadian;
|
||||
}
|
||||
|
||||
private async Task ConfirmEditOrigin()
|
||||
{
|
||||
try
|
||||
{
|
||||
// Convert yaw to radians if needed
|
||||
var yawRadians = _editOriginYawUseRadian
|
||||
? _editOriginYaw
|
||||
: _editOriginYaw * Math.PI / 180.0;
|
||||
|
||||
// Create pose with new origin position and orientation
|
||||
var q = QuaternionNumbers.FromYawRadian(yawRadians);
|
||||
var newOriginPose = new PoseDto
|
||||
{
|
||||
Position = new RobotNet10.Shared.Numbers.Vector3 { X = _editOriginX, Y = _editOriginY, Z = 0 },
|
||||
Orientation = new QuaternionGeometry(q.X, q.Y, q.Z, q.W),
|
||||
Timestamp = DateTime.UtcNow
|
||||
};
|
||||
|
||||
// Call SLAMClient to transform map origin
|
||||
var success = await SLAMClient.TransformMapOriginAsync(MapName, newOriginPose);
|
||||
|
||||
if (success)
|
||||
{
|
||||
Snackbar.Add($"Origin updated to ({_editOriginX:F3}, {_editOriginY:F3}) with yaw {yawRadians:F3} rad", Severity.Success);
|
||||
_editOriginDialogVisible = false;
|
||||
|
||||
// Reload map info to reflect changes
|
||||
await LoadMapInfoAsync();
|
||||
}
|
||||
else
|
||||
{
|
||||
Snackbar.Add("Failed to update map origin", Severity.Error);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Snackbar.Add($"Failed to update origin: {ex.Message}", Severity.Error);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Rerender Map Dialog
|
||||
|
||||
// Dialog state
|
||||
private bool _rerenderDialogVisible = false;
|
||||
private readonly DialogOptions _rerenderDialogOptions = new()
|
||||
{
|
||||
CloseOnEscapeKey = false,
|
||||
CloseButton = true,
|
||||
BackdropClick = false,
|
||||
MaxWidth = MaxWidth.Medium,
|
||||
FullWidth = true
|
||||
};
|
||||
|
||||
// Rerender config
|
||||
private OccupancyGridConfigurationDto _rerenderConfig = new();
|
||||
|
||||
private void OpenRerenderDialog()
|
||||
{
|
||||
// Reset to default values
|
||||
_rerenderConfig = new OccupancyGridConfigurationDto();
|
||||
_rerenderDialogVisible = true;
|
||||
}
|
||||
|
||||
private void CloseRerenderDialog()
|
||||
{
|
||||
_rerenderDialogVisible = false;
|
||||
}
|
||||
|
||||
private async Task ConfirmRerender()
|
||||
{
|
||||
try
|
||||
{
|
||||
// Close dialog
|
||||
_rerenderDialogVisible = false;
|
||||
|
||||
// Call SLAMClient to rerender map
|
||||
var success = await SLAMClient.RerenderMapWithConfigAsync(MapName, _rerenderConfig);
|
||||
|
||||
if (success)
|
||||
{
|
||||
Snackbar.Add("Map rerender started. Please wait...", Severity.Info);
|
||||
}
|
||||
else
|
||||
{
|
||||
Snackbar.Add("Failed to start map rerender (map may already be processing)", Severity.Error);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Snackbar.Add($"Failed to rerender map: {ex.Message}", Severity.Error);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Dispose
|
||||
|
||||
public async ValueTask DisposeAsync()
|
||||
{
|
||||
// Unsubscribe from event
|
||||
SLAMClient.IsProcessingChanged -= OnIsProcessingChanged;
|
||||
|
||||
// Unsubscribe from map processing group
|
||||
try
|
||||
{
|
||||
if (SLAMClient.IsConnected && !string.IsNullOrEmpty(MapName))
|
||||
{
|
||||
await SLAMClient.UnsubscribeMapProcessingAsync(MapName);
|
||||
}
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors during dispose
|
||||
}
|
||||
|
||||
// Stop SLAMClient
|
||||
try
|
||||
{
|
||||
await SLAMClient.StopAsync();
|
||||
}
|
||||
catch
|
||||
{
|
||||
// Ignore errors during dispose
|
||||
}
|
||||
|
||||
if (_jsModule != null)
|
||||
{
|
||||
await _jsModule.DisposeAsync();
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
.map-edit-page {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
height: 100vh;
|
||||
width: 100%;
|
||||
background-color: #1e1e1e;
|
||||
}
|
||||
|
||||
.control-bar {
|
||||
background-color: #2d2d2d;
|
||||
padding: 8px 16px;
|
||||
border-bottom: 1px solid #404040;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
|
||||
.content-area {
|
||||
display: flex;
|
||||
flex: 1;
|
||||
min-height: 0;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
.info-bar {
|
||||
width: 280px;
|
||||
background-color: #2d2d2d;
|
||||
padding: 16px;
|
||||
border-right: 1px solid #404040;
|
||||
overflow-y: auto;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
|
||||
.info-item {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
padding: 4px 0;
|
||||
}
|
||||
|
||||
.info-item-origin {
|
||||
flex-wrap: nowrap;
|
||||
}
|
||||
|
||||
/* Reuse same CSS classes as MapLocalization for consistency */
|
||||
.map-localization-container {
|
||||
background-color: #CCCCCC;
|
||||
flex: 1;
|
||||
cursor: grab;
|
||||
overflow: hidden;
|
||||
position: relative;
|
||||
min-height: 0;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
}
|
||||
|
||||
.map-localization-container:active {
|
||||
cursor: grabbing;
|
||||
}
|
||||
|
||||
.map-view-movement {
|
||||
width: fit-content;
|
||||
height: fit-content;
|
||||
position: absolute;
|
||||
cursor: default;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
.map-canvas {
|
||||
position: absolute;
|
||||
top: 0;
|
||||
left: 0;
|
||||
transform: scale(1, 1);
|
||||
transform-origin: center;
|
||||
pointer-events: none;
|
||||
image-rendering: pixelated;
|
||||
}
|
||||
|
||||
.map-editor {
|
||||
position: absolute;
|
||||
top: 0;
|
||||
left: 0;
|
||||
transform: scale(1, -1);
|
||||
transform-origin: center;
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
@implements IAsyncDisposable
|
||||
|
||||
@using MudBlazor
|
||||
@using RobotNet10.RobotApp.Client.Clients
|
||||
@using RobotNet10.RobotApp.Client.Shared.SLAM
|
||||
@using RobotNet10.Shared.Geometry
|
||||
@using RobotNet10.Shared.Localization
|
||||
@using Microsoft.JSInterop
|
||||
@using System.Linq
|
||||
|
||||
@inject SLAMClient CartographerClient
|
||||
@inject ISnackbar Snackbar
|
||||
@inject IJSRuntime JSRuntime
|
||||
|
||||
<div class="map-localization-container" @ref="containerRef" tabindex="1">
|
||||
<div @ref="viewMovementRef" class="map-view-movement">
|
||||
<!-- Canvas for base occupancy grid (background layer) -->
|
||||
<canvas @ref="mapCanvasBaseRef" class="map-canvas">
|
||||
</canvas>
|
||||
<!-- Canvas for laser scan points (middle layer) -->
|
||||
<canvas @ref="laserScanCanvasRef" class="map-canvas">
|
||||
</canvas>
|
||||
<!-- SVG overlay for robot position, trajectory, etc. (foreground layer) -->
|
||||
<svg @ref="mapContainerRef" class="map-editor" viewBox="0 0 0 0">
|
||||
<defs>
|
||||
<marker id="originvector" markerWidth="2.4" markerHeight="2.4" refX="0.1" refY="0.1">
|
||||
<line x1="0" y1="0.1" x2="0.5" y2="0.1" stroke="red" stroke-width="0.02" />
|
||||
<path d="M 0.5 0.15 L 0.6 0.1 L 0.5 0.05 Z" fill="red" stroke-width="0" />
|
||||
<line x1="0.1" y1="0" x2="0.1" y2="0.5" stroke="blue" stroke-width="0.02" />
|
||||
<path d="M 0.05 0.5 L 0.1 0.6 L 0.15 0.5 Z" fill="blue" stroke-width="0" />
|
||||
</marker>
|
||||
</defs>
|
||||
<!-- Trajectory polyline (ScanMapping) - updated via JsInvoke, no id -->
|
||||
<polyline @ref="trajectoryPolylineRef" points="" fill="none" stroke="#00FF00" stroke-width="0.05" opacity="0.6" />
|
||||
<GoalPose @ref="GoalPoseRef" />
|
||||
<!-- Robot pose component -->
|
||||
<RobotPose @ref="RobotPoseRef" />
|
||||
<!-- Grid origin marker -->
|
||||
<line class="origin" marker-end="url(#originvector)" />
|
||||
@Elements
|
||||
</svg>
|
||||
</div>
|
||||
|
||||
<MapMousePosition @ref="MapMousePositionRef" />
|
||||
<RobotPoseInfo @ref="RobotPoseInfoRef" />
|
||||
</div>
|
||||
|
||||
@code {
|
||||
[Parameter]
|
||||
public RenderFragment? Elements { get; set; }
|
||||
|
||||
private ElementReference containerRef;
|
||||
private ElementReference viewMovementRef;
|
||||
private ElementReference mapContainerRef;
|
||||
private ElementReference mapCanvasBaseRef;
|
||||
private ElementReference laserScanCanvasRef;
|
||||
private ElementReference trajectoryPolylineRef;
|
||||
private MapMousePosition MapMousePositionRef = null!;
|
||||
private GoalPose GoalPoseRef = null!;
|
||||
private RobotPose RobotPoseRef = null!;
|
||||
private RobotPoseInfo RobotPoseInfoRef = null!;
|
||||
|
||||
private bool ShowMap => CurrentGrid != null;
|
||||
}
|
||||
@@ -0,0 +1,949 @@
|
||||
using Microsoft.JSInterop;
|
||||
using RobotNet10.RobotApp.Client.Clients;
|
||||
using RobotNet10.RobotApp.Client.Shared.SLAM;
|
||||
using RobotNet10.RobotApp.Shared.Enums;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using QuaternionGeometry = RobotNet10.Shared.Geometry.Quaternion;
|
||||
using QuaternionNumbers = RobotNet10.Shared.Numbers.Quaternion;
|
||||
|
||||
namespace RobotNet10.RobotApp.Client.Components.SLAM;
|
||||
|
||||
public partial class MapLocalization
|
||||
{
|
||||
private const int GridPollIntervalMs = 3000;
|
||||
private const int PoseLaserPollIntervalMs = 500;
|
||||
private const int GridRequestMaxRetries = 10;
|
||||
private const int GridRequestRetryDelayMs = 500;
|
||||
private const double MinFitScale = 0.5;
|
||||
|
||||
private IJSObjectReference _jsModule = null!;
|
||||
private DotNetObjectReference<MapLocalization> _dotNetObj = null!;
|
||||
|
||||
// Container rect state
|
||||
private double _containerRectX = 0.0;
|
||||
private double _containerRectY = 0.0;
|
||||
private double _containerRectWidth = 0.0;
|
||||
private double _containerRectHeight = 0.0;
|
||||
private double _containerRectTop = 0.0;
|
||||
private double _containerRectRight = 0.0;
|
||||
private double _containerRectBottom = 0.0;
|
||||
private double _containerRectLeft = 0.0;
|
||||
|
||||
// View state
|
||||
public double CursorX { get; private set; } = 0.0;
|
||||
public double CursorY { get; private set; } = 0.0;
|
||||
private double _clientOriginX = 0.0;
|
||||
private double _clientOriginY = 0.0;
|
||||
private double _scale = 1.0;
|
||||
private double _left = 0.0;
|
||||
private double _top = 0.0;
|
||||
private double _fitScale = 1.0;
|
||||
|
||||
// Map data
|
||||
private double _resolution = 1.0;
|
||||
private double _originX = 0.0;
|
||||
private double _originY = 0.0; // Transformed origin Y (like MapContainer.OriginY)
|
||||
private double _mapOriginY = 0.0; // Original origin Y from grid (like MapContainer MapData.OriginY)
|
||||
private double _imageWidth = 0.0;
|
||||
private double _imageHeight = 0.0;
|
||||
|
||||
// Grid data
|
||||
public OccupancyGridDto? CurrentGrid { get; private set; }
|
||||
private OccupancyGridDto? _lastDrawnGrid; // Track last drawn grid
|
||||
private DateTime _lastGridUpdateTime = DateTime.MinValue;
|
||||
|
||||
// Trajectory path for polyline (ScanMapping only) - world coordinates for SVG viewBox
|
||||
private List<Vector2> _trajectoryPath = [];
|
||||
|
||||
// SVG viewBox origin (similar to MapContainer.OriginX and OriginY)
|
||||
private double _svgOriginX = 0.0; // SVG viewBox X origin (world coordinates)
|
||||
private double _svgOriginY = 0.0; // SVG viewBox Y origin (world coordinates)
|
||||
|
||||
// State
|
||||
private SLAMState? _currentState;
|
||||
/// <summary>True when grid/pose/laser should be requested and displayed (Localizing or ScanMapping).</summary>
|
||||
private bool IsMapDisplayActive => _currentState == SLAMState.Relocalizing || _currentState == SLAMState.Localizing || _currentState == SLAMState.ScanMapping;
|
||||
/// <summary>Reference grid for view.</summary>
|
||||
private OccupancyGridDto? ReferenceGrid => CurrentGrid;
|
||||
private System.Timers.Timer? _updateTimer;
|
||||
private System.Timers.Timer? _poseLaserTimer; // Timer for pose and laser scan updates (0.5s)
|
||||
private readonly Lock _timerLock = new();
|
||||
private readonly Lock _poseLaserTimerLock = new();
|
||||
private bool _hasRequestedGridForLocalization = false;
|
||||
|
||||
// Robot pose and laser scan data
|
||||
private PoseDto _currentPose = new()
|
||||
{
|
||||
Position = new RobotNet10.Shared.Numbers.Vector3 { X = 0, Y = 0, Z = 0 },
|
||||
Orientation = new QuaternionGeometry { W = 1, X = 0, Y = 0, Z = 0 },
|
||||
Timestamp = DateTime.UtcNow
|
||||
};
|
||||
private RobotNet10.Shared.Numbers.Vector3[]? _currentLaserScanPoints;
|
||||
|
||||
#region Lifecycle
|
||||
|
||||
protected override async Task OnAfterRenderAsync(bool firstRender)
|
||||
{
|
||||
await base.OnAfterRenderAsync(firstRender);
|
||||
if (!firstRender) return;
|
||||
|
||||
// Load JavaScript module (canvas + view/events)
|
||||
_jsModule = await JSRuntime.InvokeAsync<IJSObjectReference>("import", "/js/mapLocalization.js");
|
||||
|
||||
// Setup zoom and pan event listeners
|
||||
_dotNetObj = DotNetObjectReference.Create(this);
|
||||
await _jsModule.InvokeVoidAsync("updateContainerRect", _dotNetObj, containerRef, nameof(OnContainerResize));
|
||||
await _jsModule.InvokeVoidAsync("registerResizeObserver", _dotNetObj, containerRef, nameof(OnContainerResize));
|
||||
await _jsModule.InvokeVoidAsync("addMouseWheelEventListener", _dotNetObj, containerRef, nameof(OnMouseWheel));
|
||||
await _jsModule.InvokeVoidAsync("addMouseMoveEventListener", _dotNetObj, containerRef, nameof(OnMouseMove));
|
||||
await _jsModule.InvokeVoidAsync("addMouseDownEventListener", _dotNetObj, containerRef, nameof(OnMouseDown));
|
||||
await _jsModule.InvokeVoidAsync("addMouseUpEventListener", _dotNetObj, containerRef, nameof(OnMouseUp));
|
||||
|
||||
await CartographerClient.StartAsync();
|
||||
|
||||
// Get initial state and trigger OnStateChanged to handle grid loading and timer setup
|
||||
var state = await CartographerClient.GetCurrentStateAsync();
|
||||
OnStateChanged(state);
|
||||
|
||||
// Subscribe to events
|
||||
CartographerClient.StateChanged += OnStateChanged;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region State & Grid
|
||||
|
||||
private void OnStateChanged(SLAMState state)
|
||||
{
|
||||
_currentState = state;
|
||||
UpdateTimerBasedOnState();
|
||||
|
||||
// When entering InitializingLocalizing, Localizing or ScanMapping state, request OccupancyGrid once
|
||||
if ((state == SLAMState.Relocalizing || state == SLAMState.Localizing || state == SLAMState.ScanMapping) && !_hasRequestedGridForLocalization)
|
||||
{
|
||||
_hasRequestedGridForLocalization = true;
|
||||
_ = InvokeAsync(async () =>
|
||||
{
|
||||
await RequestOccupancyGridForLocalizationAsync();
|
||||
// Start pose/laser scan timer after requesting grid
|
||||
UpdatePoseLaserTimerBasedOnState();
|
||||
});
|
||||
}
|
||||
else if (state == SLAMState.Relocalizing || state == SLAMState.Localizing || state == SLAMState.ScanMapping)
|
||||
{
|
||||
// Already requested grid, only update timer if grid is available
|
||||
// This handles state transitions like Relocalizing -> Localizing where grid request is still in progress
|
||||
if (CurrentGrid != null)
|
||||
{
|
||||
UpdatePoseLaserTimerBasedOnState();
|
||||
}
|
||||
// If CurrentGrid is null, the timer will be started by the InvokeAsync above when grid is loaded
|
||||
}
|
||||
else
|
||||
{
|
||||
UpdatePoseLaserTimerBasedOnState();
|
||||
}
|
||||
|
||||
// Reset flag when leaving map-display states (InitializingLocalizing, Localizing, ScanMapping)
|
||||
if (state != SLAMState.Relocalizing && state != SLAMState.Localizing && state != SLAMState.ScanMapping)
|
||||
{
|
||||
_hasRequestedGridForLocalization = false;
|
||||
_trajectoryPath.Clear();
|
||||
_ = InvokeAsync(async () =>
|
||||
{
|
||||
await ClearLaserScanAsync();
|
||||
await UpdateTrajectoryPolylineAsync(); // Clear polyline via JsInvoke
|
||||
RobotPoseRef.UpdatePose(0, 0, 0);
|
||||
RobotPoseInfoRef.Update(0, 0, 0, 0);
|
||||
});
|
||||
}
|
||||
else if (state == SLAMState.ScanMapping)
|
||||
{
|
||||
// Entering ScanMapping: clear trajectory until timer fetches nodes
|
||||
_trajectoryPath.Clear();
|
||||
_ = InvokeAsync(UpdateTrajectoryPolylineAsync);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Grid & Map
|
||||
|
||||
private async Task LoadMapFromGrid(OccupancyGridDto grid)
|
||||
{
|
||||
CurrentGrid = grid;
|
||||
_resolution = grid.Resolution;
|
||||
_originX = grid.Origin.Position.X;
|
||||
_mapOriginY = grid.Origin.Position.Y; // Original origin Y from grid
|
||||
// Similar to MapContainer: OriginY = -ImageHeight * Resolution - mapData.OriginY
|
||||
_originY = -grid.Height * grid.Resolution - grid.Origin.Position.Y;
|
||||
_imageWidth = grid.Width * grid.Resolution;
|
||||
_imageHeight = grid.Height * grid.Resolution;
|
||||
|
||||
// Update SVG origin from grid origin
|
||||
UpdateSvgOrigin();
|
||||
|
||||
// Draw the grid on canvas
|
||||
await DrawOccupancyGrid();
|
||||
}
|
||||
|
||||
private void UpdateSvgOrigin()
|
||||
{
|
||||
var referenceGrid = ReferenceGrid;
|
||||
if (referenceGrid == null)
|
||||
{
|
||||
_svgOriginX = 0.0;
|
||||
_svgOriginY = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
// SVG viewBox origin uses world coordinates from grid origin
|
||||
_svgOriginX = referenceGrid.Origin.Position.X;
|
||||
_svgOriginY = referenceGrid.Origin.Position.Y;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Apply origin and image size from a grid.
|
||||
/// </summary>
|
||||
private void ApplyOriginFromGrid(OccupancyGridDto grid)
|
||||
{
|
||||
_resolution = grid.Resolution;
|
||||
_originX = grid.Origin.Position.X;
|
||||
_mapOriginY = grid.Origin.Position.Y;
|
||||
_originY = -grid.Height * grid.Resolution - grid.Origin.Position.Y;
|
||||
_imageWidth = grid.Width * grid.Resolution;
|
||||
_imageHeight = grid.Height * grid.Resolution;
|
||||
UpdateSvgOrigin();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Apply trajectory from grid DTO to _trajectoryPath and update polyline if changed.
|
||||
/// </summary>
|
||||
/// <returns>True if trajectory was updated.</returns>
|
||||
private async Task<bool> TryApplyTrajectoryFromGrid(OccupancyGridDto? grid)
|
||||
{
|
||||
if (grid?.TrajectoryNodes == null || grid.TrajectoryNodes.Length == 0)
|
||||
return false;
|
||||
var newPath = grid.TrajectoryNodes.Select(n => new Vector2(n.Pose.Position.X, n.Pose.Position.Y)).ToList();
|
||||
if (newPath.Count == _trajectoryPath.Count && newPath.SequenceEqual(_trajectoryPath))
|
||||
return false;
|
||||
_trajectoryPath = newPath;
|
||||
await UpdateTrajectoryPolylineAsync();
|
||||
return true;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Drawing
|
||||
|
||||
/// <summary>
|
||||
/// Create a snapshot of grid for _lastDrawnGrid* tracking.
|
||||
/// </summary>
|
||||
private static OccupancyGridDto CreateDrawnGridSnapshot(OccupancyGridDto grid)
|
||||
{
|
||||
return new OccupancyGridDto
|
||||
{
|
||||
Resolution = grid.Resolution,
|
||||
Width = grid.Width,
|
||||
Height = grid.Height,
|
||||
Origin = grid.Origin,
|
||||
Version = grid.Version,
|
||||
KnownCells = grid.KnownCells ?? [],
|
||||
LastBaseUpdated = grid.LastBaseUpdated,
|
||||
LastUpdated = grid.LastUpdated,
|
||||
TrajectoryNodes = grid.TrajectoryNodes
|
||||
};
|
||||
}
|
||||
|
||||
private async Task DrawOccupancyGrid()
|
||||
{
|
||||
var referenceGrid = ReferenceGrid;
|
||||
if (referenceGrid == null) return;
|
||||
|
||||
var grid = CurrentGrid;
|
||||
|
||||
// Check if we need to redraw grid
|
||||
bool needsRedraw = false;
|
||||
if (grid != null && (_lastDrawnGrid == null || _lastDrawnGrid.Version != grid.Version))
|
||||
{
|
||||
needsRedraw = true;
|
||||
}
|
||||
|
||||
if (!needsRedraw)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
// Get container size to calculate scale for auto-fit
|
||||
await Task.Delay(10);
|
||||
var containerSize = await _jsModule.InvokeAsync<double[]>("getElementSize", containerRef);
|
||||
var containerWidth = containerSize[0];
|
||||
var containerHeight = containerSize[1];
|
||||
|
||||
if (containerWidth <= 0 || containerHeight <= 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Use reference grid for fit scale (base or updating)
|
||||
var refW = referenceGrid.Width;
|
||||
var refH = referenceGrid.Height;
|
||||
if (_imageWidth <= 0 || _imageHeight <= 0)
|
||||
{
|
||||
_imageWidth = refW * referenceGrid.Resolution;
|
||||
_imageHeight = refH * referenceGrid.Resolution;
|
||||
_originX = referenceGrid.Origin.Position.X;
|
||||
_mapOriginY = referenceGrid.Origin.Position.Y;
|
||||
_originY = -referenceGrid.Height * referenceGrid.Resolution - referenceGrid.Origin.Position.Y;
|
||||
}
|
||||
if (_imageWidth > 0 && _imageHeight > 0)
|
||||
{
|
||||
_fitScale = Math.Min(containerWidth / _imageWidth, containerHeight / _imageHeight);
|
||||
}
|
||||
else
|
||||
{
|
||||
var scaleX = containerWidth / refW;
|
||||
var scaleY = containerHeight / refH;
|
||||
_fitScale = Math.Min(scaleX, scaleY);
|
||||
}
|
||||
|
||||
if (_fitScale < MinFitScale)
|
||||
_fitScale = MinFitScale;
|
||||
if (_scale <= 0)
|
||||
_scale = _fitScale;
|
||||
|
||||
// Draw grid
|
||||
if (needsRedraw && grid != null)
|
||||
{
|
||||
await _jsModule.InvokeVoidAsync("setCanvasSize", mapCanvasBaseRef, grid.Width, grid.Height);
|
||||
if (grid.KnownCells != null && grid.KnownCells.Length > 0)
|
||||
{
|
||||
await _jsModule.InvokeVoidAsync("drawOccupancyGrid",
|
||||
mapCanvasBaseRef, grid.Width, grid.Height, grid.KnownCells);
|
||||
}
|
||||
else
|
||||
{
|
||||
await _jsModule.InvokeVoidAsync("clearCanvas", mapCanvasBaseRef);
|
||||
}
|
||||
_lastDrawnGrid = CreateDrawnGridSnapshot(grid);
|
||||
}
|
||||
|
||||
var svgWidth = referenceGrid.Width * referenceGrid.Resolution;
|
||||
var svgHeight = referenceGrid.Height * referenceGrid.Resolution;
|
||||
await _jsModule.InvokeVoidAsync("setSvgConfig", mapContainerRef, svgWidth, svgHeight, _svgOriginX, _svgOriginY);
|
||||
|
||||
await ScaleFitContentAsync();
|
||||
|
||||
if (IsMapDisplayActive)
|
||||
{
|
||||
await UpdateRobotPoseSvgAsync();
|
||||
if (_currentLaserScanPoints != null && _currentLaserScanPoints.Length > 0)
|
||||
await DrawLaserScanAsync();
|
||||
if (_currentState == SLAMState.ScanMapping)
|
||||
await UpdateTrajectoryPolylineAsync();
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.Error.WriteLine($"[MapLocalization] DrawOccupancyGrid: Exception - {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region View & Scale
|
||||
|
||||
/// <summary>
|
||||
/// Public method to fit and center the map view.
|
||||
/// Can be called from external components/pages.
|
||||
/// </summary>
|
||||
public async Task FitViewAsync()
|
||||
{
|
||||
if (ReferenceGrid == null) return;
|
||||
|
||||
// Recalculate fit scale based on current container and image dimensions
|
||||
if (_imageWidth > 0 && _imageHeight > 0 && _containerRectWidth > 0 && _containerRectHeight > 0)
|
||||
{
|
||||
_fitScale = Math.Min(_containerRectWidth / _imageWidth, _containerRectHeight / _imageHeight);
|
||||
if (_fitScale < MinFitScale)
|
||||
_fitScale = MinFitScale;
|
||||
}
|
||||
|
||||
await ScaleFitContentAsync();
|
||||
}
|
||||
|
||||
private void UpdateClientOrigin()
|
||||
{
|
||||
if (ReferenceGrid == null) return;
|
||||
|
||||
// Calculate client origin (exactly like MapContainer.SetViewMovement and ViewContainerResize)
|
||||
// MapContainer: ClientOriginX = ViewContainerRectLeft + Left - OriginX * Scale
|
||||
// MapContainer: ClientOriginY = ViewContainerRectTop + Top - OriginY * Scale
|
||||
//
|
||||
// MapLocalization now has the same structure as MapContainer:
|
||||
// - scaleY(-1) is on SVG (class="map-editor"), not on wrapper div
|
||||
// - This matches MapContainer's structure exactly
|
||||
_clientOriginX = _containerRectLeft + _left - _originX * _scale;
|
||||
_clientOriginY = _containerRectTop + _top - _originY * _scale;
|
||||
}
|
||||
|
||||
public async Task ScaleFitContentAsync()
|
||||
{
|
||||
if (ReferenceGrid == null) return;
|
||||
|
||||
_scale = _fitScale;
|
||||
|
||||
var wrapperWidth = _imageWidth * _scale;
|
||||
var wrapperHeight = _imageHeight * _scale;
|
||||
|
||||
var centerLeft = (_containerRectWidth - wrapperWidth) / 2;
|
||||
var centerTop = (_containerRectHeight - wrapperHeight) / 2;
|
||||
|
||||
await SetViewMovement(centerLeft, centerTop);
|
||||
|
||||
await _jsModule.InvokeVoidAsync("setSvgRect", mapContainerRef, wrapperWidth, wrapperHeight);
|
||||
await _jsModule.InvokeVoidAsync("setCanvasRect", mapCanvasBaseRef, wrapperWidth, wrapperHeight);
|
||||
await _jsModule.InvokeVoidAsync("setCanvasRect", laserScanCanvasRef, wrapperWidth, wrapperHeight);
|
||||
}
|
||||
|
||||
private async Task SetViewMovement(double left, double top)
|
||||
{
|
||||
_top = top;
|
||||
_left = left;
|
||||
|
||||
// Update client origin (exactly like MapContainer.SetViewMovement)
|
||||
UpdateClientOrigin();
|
||||
|
||||
if (_jsModule != null && ReferenceGrid != null)
|
||||
{
|
||||
var width = _imageWidth * _scale;
|
||||
var height = _imageHeight * _scale;
|
||||
await _jsModule.InvokeVoidAsync("setMapMovement", viewMovementRef, _top, _left, width, height);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Event Handlers (JSInvokable)
|
||||
|
||||
[JSInvokable]
|
||||
public void OnContainerResize(double x, double y, double width, double height, double top, double right, double bottom, double left)
|
||||
{
|
||||
_containerRectX = x;
|
||||
_containerRectY = y;
|
||||
_containerRectWidth = width;
|
||||
_containerRectHeight = height;
|
||||
_containerRectTop = top;
|
||||
_containerRectRight = right;
|
||||
_containerRectBottom = bottom;
|
||||
_containerRectLeft = left;
|
||||
|
||||
// Update client origin (exactly like MapContainer.ViewContainerResize)
|
||||
UpdateClientOrigin();
|
||||
|
||||
// Recalculate fit scale (exactly like MapContainer.ViewContainerResize)
|
||||
// MapContainer: FitScale = Math.Min(ViewContainerRectWidth / ImageWidth, ViewContainerRectHeight / ImageHeight)
|
||||
if (_imageWidth > 0 && _imageHeight > 0)
|
||||
{
|
||||
_fitScale = Math.Min(_containerRectWidth / _imageWidth, _containerRectHeight / _imageHeight);
|
||||
}
|
||||
}
|
||||
|
||||
[JSInvokable]
|
||||
public async Task OnMouseWheel(double deltaY, double clientX, double clientY)
|
||||
{
|
||||
if (ReferenceGrid == null) return;
|
||||
|
||||
// Calculate scale change (exactly like MapContainer.MouseWheelOnMapContainer)
|
||||
double scaleChange;
|
||||
if (deltaY > 0)
|
||||
{
|
||||
if (_scale <= _fitScale / 2) return;
|
||||
scaleChange = _scale > _fitScale ? -(_scale / _fitScale) : -0.1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (_scale >= _fitScale * 100) return;
|
||||
scaleChange = _scale < _fitScale ? 0.5 : (_scale / _fitScale);
|
||||
}
|
||||
|
||||
// Store old scale before updating (exactly like MapContainer)
|
||||
double oldScale = _scale;
|
||||
|
||||
// Update scale (exactly like MapContainer: Scale += scaleChange)
|
||||
_scale += scaleChange;
|
||||
|
||||
// Set SVG rect first (exactly like MapContainer: ImageWidth * Scale, ImageHeight * Scale)
|
||||
await _jsModule.InvokeVoidAsync("setSvgRect", mapContainerRef, _imageWidth * _scale, _imageHeight * _scale);
|
||||
|
||||
await _jsModule.InvokeVoidAsync("setCanvasRect", mapCanvasBaseRef, _imageWidth * _scale, _imageHeight * _scale);
|
||||
await _jsModule.InvokeVoidAsync("setCanvasRect", laserScanCanvasRef, _imageWidth * _scale, _imageHeight * _scale);
|
||||
|
||||
// Calculate cursor position in world coordinates (exactly like MapContainer)
|
||||
// MapContainer: CursorX = (clientX - ClientOriginX) / Scale
|
||||
// MapContainer: CursorY = (ClientOriginY - clientY) / Scale
|
||||
// Update CursorX/CursorY (they are in world coordinates)
|
||||
CursorX = (clientX - _clientOriginX) / oldScale;
|
||||
CursorY = (_clientOriginY - clientY) / oldScale;
|
||||
MapMousePositionRef.Update(CursorX, CursorY);
|
||||
|
||||
// Calculate mouse position relative to map origin (exactly like MapContainer)
|
||||
// MapContainer: mouseX = CursorX - OriginX
|
||||
// MapContainer: mouseY = CursorY - MapData.OriginY (use original origin Y, not transformed)
|
||||
double mouseX = CursorX - _originX;
|
||||
double mouseY = CursorY - _mapOriginY;
|
||||
|
||||
// Calculate movement adjustment (exactly like MapContainer.MouseWheelOnMapContainer)
|
||||
// MapContainer: Left - mouseX * scaleChange, Top - (ImageHeight - mouseY) * scaleChange
|
||||
await SetViewMovement(_left - mouseX * scaleChange, _top - (_imageHeight - mouseY) * scaleChange);
|
||||
|
||||
// Update robot pose after scale change
|
||||
if (IsMapDisplayActive)
|
||||
{
|
||||
await UpdateRobotPoseSvgAsync();
|
||||
}
|
||||
}
|
||||
|
||||
[JSInvokable]
|
||||
public async Task OnMouseMove(double clientX, double clientY, long buttons, bool ctrlKey, double movementX, double movementY)
|
||||
{
|
||||
// Calculate cursor position in world coordinates (exactly like MapContainer.MouseMoveOnMapContainer)
|
||||
// MapContainer: CursorX = (clientX - ClientOriginX) / Scale (world coordinates in meters)
|
||||
// MapContainer: CursorY = (ClientOriginY - clientY) / Scale (world coordinates in meters)
|
||||
CursorX = (clientX - _clientOriginX) / _scale;
|
||||
CursorY = (_clientOriginY - clientY) / _scale;
|
||||
MapMousePositionRef.Update(CursorX, CursorY);
|
||||
|
||||
// Update RobotGoalPose mouse position (line goal->mouse and circle)
|
||||
if (buttons == 1) // Right mouse button down: update goal pose to current mouse
|
||||
{
|
||||
GoalPoseRef?.SetMousePosition(CursorX, CursorY);
|
||||
}
|
||||
else if (buttons == 4) // Middle mouse button
|
||||
{
|
||||
await SetViewMovement(_left + movementX, _top + movementY);
|
||||
|
||||
// Update robot pose after pan (pose position in SVG doesn't change, but we update to ensure consistency)
|
||||
if (IsMapDisplayActive)
|
||||
{
|
||||
await UpdateRobotPoseSvgAsync();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[JSInvokable]
|
||||
public async Task OnMouseDown(int button, bool altKey, bool ctrlKey, bool shiftKey)
|
||||
{
|
||||
Console.WriteLine($"OnMouseDown: button={button}, altKey={altKey}, ctrlKey={ctrlKey}, shiftKey={shiftKey}");
|
||||
if (button == 0) // Right mouse button: set robot pose and goal pose for RobotGoalPose
|
||||
{
|
||||
var robotX = _currentPose.Position.X;
|
||||
var robotY = _currentPose.Position.Y;
|
||||
GoalPoseRef?.SetRobotPosition(robotX, robotY);
|
||||
GoalPoseRef?.SetGoalPosition(CursorX, CursorY);
|
||||
GoalPoseRef?.SetMousePosition(CursorX, CursorY);
|
||||
GoalPoseRef?.Show();
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
[JSInvokable]
|
||||
public async Task OnMouseUp(int button, bool altKey, bool ctrlKey, bool shiftKey)
|
||||
{
|
||||
if (button == 0) // Right mouse button up: áp dụng Optimize (ẩn nếu goal–mouse < 1 m, ngược lại clamp 1 m)
|
||||
GoalPoseRef?.Optimize();
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Timers
|
||||
|
||||
/// <summary>
|
||||
/// Localizing: grid base requested once only (no timer).
|
||||
/// ScanMapping: timer 3s to poll grid base + grid updating + trajectory nodes.
|
||||
/// </summary>
|
||||
private void UpdateTimerBasedOnState()
|
||||
{
|
||||
lock (_timerLock)
|
||||
{
|
||||
if (_currentState == SLAMState.ScanMapping)
|
||||
{
|
||||
if (_updateTimer == null)
|
||||
{
|
||||
_updateTimer = new System.Timers.Timer(GridPollIntervalMs)
|
||||
{
|
||||
AutoReset = false
|
||||
};
|
||||
_updateTimer.Elapsed += OnTimerElapsed;
|
||||
_updateTimer.Start();
|
||||
}
|
||||
else if (!_updateTimer.Enabled)
|
||||
{
|
||||
_updateTimer.Start();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (_updateTimer != null)
|
||||
{
|
||||
_updateTimer.Stop();
|
||||
_updateTimer.Elapsed -= OnTimerElapsed;
|
||||
_updateTimer.Dispose();
|
||||
_updateTimer = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
private void OnTimerElapsed(object? sender, System.Timers.ElapsedEventArgs e)
|
||||
{
|
||||
_ = InvokeAsync(async () =>
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_currentState != SLAMState.ScanMapping)
|
||||
{
|
||||
lock (_timerLock)
|
||||
{
|
||||
if (_updateTimer != null && _currentState == SLAMState.ScanMapping)
|
||||
_updateTimer.Start();
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
bool hasUpdates = false;
|
||||
|
||||
var grid = await CartographerClient.GetOccupancyGridAsync(_lastGridUpdateTime);
|
||||
if (grid != null)
|
||||
{
|
||||
CurrentGrid = grid;
|
||||
_lastGridUpdateTime = grid.LastBaseUpdated;
|
||||
hasUpdates = true;
|
||||
UpdateSvgOrigin();
|
||||
await TryApplyTrajectoryFromGrid(grid);
|
||||
}
|
||||
|
||||
if (hasUpdates && ReferenceGrid != null)
|
||||
{
|
||||
// BUG FIX: Always update origin when grid changes, not just when dimensions change significantly
|
||||
// Grid origin can change when grid grows even if dimensions stay similar
|
||||
if (CurrentGrid != null)
|
||||
{
|
||||
// Check if origin changed (which indicates grid grow/shift)
|
||||
var originChanged = Math.Abs(_originX - CurrentGrid.Origin.Position.X) > 0.0001 ||
|
||||
Math.Abs(_mapOriginY - CurrentGrid.Origin.Position.Y) > 0.0001;
|
||||
|
||||
// Check if dimensions changed significantly
|
||||
var dimensionsChanged = _imageWidth == 0 || _imageHeight == 0 ||
|
||||
Math.Abs(_imageWidth - CurrentGrid.Width * CurrentGrid.Resolution) > 0.001 ||
|
||||
Math.Abs(_imageHeight - CurrentGrid.Height * CurrentGrid.Resolution) > 0.001;
|
||||
|
||||
if (dimensionsChanged || originChanged)
|
||||
{
|
||||
Console.WriteLine($"[MapLocalization] Grid changed: dims={dimensionsChanged}, origin={originChanged}, " +
|
||||
$"size={CurrentGrid.Width}x{CurrentGrid.Height}, " +
|
||||
$"origin=[{CurrentGrid.Origin.Position.X:F3},{CurrentGrid.Origin.Position.Y:F3}], " +
|
||||
$"prevOrigin=[{_originX:F3},{_mapOriginY:F3}]");
|
||||
await LoadMapFromGrid(CurrentGrid);
|
||||
}
|
||||
else
|
||||
await DrawOccupancyGrid();
|
||||
}
|
||||
StateHasChanged();
|
||||
}
|
||||
|
||||
lock (_timerLock)
|
||||
{
|
||||
if (_updateTimer != null && _currentState == SLAMState.ScanMapping)
|
||||
_updateTimer.Start();
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.Error.WriteLine($"[MapLocalization] Error in OnTimerElapsed: {ex.Message}");
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
private async Task RequestOccupancyGridForLocalizationAsync()
|
||||
{
|
||||
try
|
||||
{
|
||||
var requestTime = DateTime.MinValue;
|
||||
for (int i = 0; i < GridRequestMaxRetries; i++)
|
||||
{
|
||||
if (!IsMapDisplayActive)
|
||||
break;
|
||||
|
||||
var grid = await CartographerClient.GetOccupancyGridAsync(requestTime);
|
||||
if (grid == null)
|
||||
{
|
||||
await Task.Delay(GridRequestRetryDelayMs);
|
||||
continue;
|
||||
}
|
||||
|
||||
CurrentGrid = grid;
|
||||
_lastGridUpdateTime = grid.LastBaseUpdated;
|
||||
await LoadMapFromGrid(grid);
|
||||
await TryApplyTrajectoryFromGrid(grid);
|
||||
break;
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.Error.WriteLine($"[MapLocalization] Error requesting OccupancyGrid for localization: {ex.Message}");
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
private void UpdatePoseLaserTimerBasedOnState()
|
||||
{
|
||||
lock (_poseLaserTimerLock)
|
||||
{
|
||||
if (IsMapDisplayActive)
|
||||
{
|
||||
if (_poseLaserTimer == null)
|
||||
{
|
||||
_poseLaserTimer = new System.Timers.Timer(PoseLaserPollIntervalMs)
|
||||
{
|
||||
AutoReset = false
|
||||
};
|
||||
_poseLaserTimer.Elapsed += OnPoseLaserTimerElapsed;
|
||||
_poseLaserTimer.Start();
|
||||
}
|
||||
else if (!_poseLaserTimer.Enabled)
|
||||
{
|
||||
_poseLaserTimer.Start();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (_poseLaserTimer != null)
|
||||
{
|
||||
_poseLaserTimer.Stop();
|
||||
_poseLaserTimer.Elapsed -= OnPoseLaserTimerElapsed;
|
||||
_poseLaserTimer.Dispose();
|
||||
_poseLaserTimer = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void OnPoseLaserTimerElapsed(object? sender, System.Timers.ElapsedEventArgs e)
|
||||
{
|
||||
_ = InvokeAsync(async () =>
|
||||
{
|
||||
try
|
||||
{
|
||||
// Get robot pose
|
||||
var pose = await CartographerClient.GetCurrentPoseAsync();
|
||||
if (pose != null)
|
||||
{
|
||||
_currentPose = pose;
|
||||
await UpdateRobotPoseSvgAsync();
|
||||
}
|
||||
|
||||
// Get laser scan
|
||||
var laserScanPoints = await CartographerClient.GetSamplePointCloudAsync();
|
||||
if (laserScanPoints != null && laserScanPoints.Length > 0)
|
||||
{
|
||||
_currentLaserScanPoints = laserScanPoints;
|
||||
var refGrid = ReferenceGrid;
|
||||
if (refGrid != null && refGrid.Width > 0 && refGrid.Height > 0)
|
||||
await DrawLaserScanAsync();
|
||||
}
|
||||
|
||||
lock (_poseLaserTimerLock)
|
||||
{
|
||||
if (_poseLaserTimer != null && IsMapDisplayActive)
|
||||
{
|
||||
_poseLaserTimer.Start();
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.Error.WriteLine($"[MapLocalization] Error in OnPoseLaserTimerElapsed: {ex.Message}");
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
#region Helpers
|
||||
|
||||
/// <summary>
|
||||
/// Lấy goal pose từ RobotGoalPose (GoalX, GoalY, GoalYaw) dưới dạng PoseDto để gửi SetInitialPose.
|
||||
/// </summary>
|
||||
public PoseDto? GetGoalPoseDto()
|
||||
{
|
||||
if (GoalPoseRef == null)
|
||||
return null;
|
||||
var q = QuaternionNumbers.FromYawRadian(GoalPoseRef.GoalYaw);
|
||||
return new PoseDto
|
||||
{
|
||||
Position = new RobotNet10.Shared.Numbers.Vector3 { X = GoalPoseRef.GoalX, Y = GoalPoseRef.GoalY, Z = 0 },
|
||||
Orientation = new QuaternionGeometry(q.X, q.Y, q.Z, q.W),
|
||||
Timestamp = DateTime.UtcNow
|
||||
};
|
||||
}
|
||||
|
||||
private async Task UpdateRobotPoseSvgAsync()
|
||||
{
|
||||
try
|
||||
{
|
||||
// Hide robot pose if scale is invalid or no grid (base or updating for ScanMapping)
|
||||
var referenceGrid = CurrentGrid;
|
||||
if (_scale <= 0 || referenceGrid == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var robotX = _currentPose.Position.X;
|
||||
var robotY = _currentPose.Position.Y;
|
||||
var yaw = _currentPose.Orientation.ToYawRadian();
|
||||
|
||||
RobotPoseRef.UpdatePose(robotX, robotY, yaw);
|
||||
RobotPoseInfoRef.Update(robotX, robotY, yaw, _currentPose.Score);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.Error.WriteLine($"[MapLocalization] UpdateRobotPoseSvgAsync: Exception - {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
private (int x, int y) WorldToGrid(double worldX, double worldY)
|
||||
{
|
||||
var grid = ReferenceGrid;
|
||||
if (grid == null)
|
||||
return (0, 0);
|
||||
|
||||
var relativeX = worldX - grid.Origin.Position.X;
|
||||
var relativeY = worldY - grid.Origin.Position.Y;
|
||||
|
||||
var gridX = (int)Math.Floor(relativeX / grid.Resolution);
|
||||
var gridY = (int)Math.Floor(relativeY / grid.Resolution);
|
||||
|
||||
return (gridX, gridY);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Update trajectory polyline via JsInvoke (ElementReference, no StateHasChanged).
|
||||
/// Only updates points data; other attributes (fill, stroke, stroke-width, opacity) are fixed in markup.
|
||||
/// </summary>
|
||||
private async Task UpdateTrajectoryPolylineAsync()
|
||||
{
|
||||
var pointsStr = "";
|
||||
if (_trajectoryPath.Count > 1)
|
||||
{
|
||||
var worldPoints = _trajectoryPath.Select(p => $"{p.X},{p.Y}");
|
||||
pointsStr = string.Join(" ", worldPoints);
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await _jsModule.InvokeVoidAsync("setPolylinePointsOnly",
|
||||
trajectoryPolylineRef,
|
||||
pointsStr);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.Error.WriteLine($"[MapLocalization] UpdateTrajectoryPolylineAsync: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
private async Task DrawLaserScanAsync()
|
||||
{
|
||||
var referenceGrid = ReferenceGrid;
|
||||
if (_currentLaserScanPoints == null || _currentLaserScanPoints.Length == 0 || referenceGrid == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (referenceGrid.Width <= 0 || referenceGrid.Height <= 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Set canvas size
|
||||
await _jsModule.InvokeVoidAsync("setCanvasSize", laserScanCanvasRef, referenceGrid.Width, referenceGrid.Height);
|
||||
|
||||
// laserScanPoints from GetSamplePointCloudAsync() are already in global (map) coordinates.
|
||||
// Only need to convert to grid coordinates via WorldToGrid (uses grid.Origin).
|
||||
// laserScanCanvasRef uses class "map-canvas" with CSS transform: scale(1, -1), so the canvas
|
||||
// is rendered with Y flipped: buffer y=0 (top) appears at bottom, buffer y=height-1 (bottom) at top.
|
||||
// We pass (gridX, gridY) directly: grid Y increases upward. Drawing at (gridX, gridY) puts
|
||||
// world-Y-up at large buffer y; after scale(1,-1) that displays at visual top. Correct.
|
||||
var mapPoints = new List<(double X, double Y)>();
|
||||
|
||||
foreach (var point in _currentLaserScanPoints)
|
||||
{
|
||||
// point.X, point.Y are already world coordinates; WorldToGrid applies Origin transform
|
||||
var (gridX, gridY) = WorldToGrid(point.X, point.Y);
|
||||
|
||||
if (gridX >= 0 && gridX < referenceGrid.Width && gridY >= 0 && gridY < referenceGrid.Height)
|
||||
{
|
||||
// Pass grid coords (Y-up); map-canvas CSS scale(1,-1) handles display flip
|
||||
mapPoints.Add((gridX, gridY));
|
||||
}
|
||||
}
|
||||
|
||||
if (mapPoints.Count > 0)
|
||||
{
|
||||
var pointsArray = mapPoints.Select(p => new double[] { p.X, p.Y }).ToArray();
|
||||
await _jsModule.InvokeVoidAsync("drawPointsOnCanvas", laserScanCanvasRef, pointsArray, "#FF0000", 0.5);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.Error.WriteLine($"[MapLocalization] DrawLaserScanAsync: Exception - {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
private async Task ClearLaserScanAsync()
|
||||
{
|
||||
await _jsModule.InvokeVoidAsync("clearCanvas", laserScanCanvasRef);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Dispose
|
||||
|
||||
public async ValueTask DisposeAsync()
|
||||
{
|
||||
CartographerClient.StateChanged -= OnStateChanged;
|
||||
|
||||
lock (_timerLock)
|
||||
{
|
||||
if (_updateTimer != null)
|
||||
{
|
||||
_updateTimer.Stop();
|
||||
_updateTimer.Elapsed -= OnTimerElapsed;
|
||||
_updateTimer.Dispose();
|
||||
_updateTimer = null;
|
||||
}
|
||||
}
|
||||
|
||||
lock (_poseLaserTimerLock)
|
||||
{
|
||||
if (_poseLaserTimer != null)
|
||||
{
|
||||
_poseLaserTimer.Stop();
|
||||
_poseLaserTimer.Elapsed -= OnPoseLaserTimerElapsed;
|
||||
_poseLaserTimer.Dispose();
|
||||
_poseLaserTimer = null;
|
||||
}
|
||||
}
|
||||
|
||||
await _jsModule.DisposeAsync();
|
||||
_dotNetObj?.Dispose();
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
.map-localization-container {
|
||||
background-color: #bfbfbf;
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
cursor: not-allowed;
|
||||
border-top: solid 2px #808080;
|
||||
overflow: hidden;
|
||||
position: relative;
|
||||
min-height: 0;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
}
|
||||
|
||||
.map-view-movement {
|
||||
width: fit-content;
|
||||
height: fit-content;
|
||||
position: absolute;
|
||||
cursor: default;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
.map-canvas {
|
||||
position: absolute;
|
||||
top: 0;
|
||||
left: 0;
|
||||
transform: scale(1, -1);
|
||||
transform-origin: center;
|
||||
pointer-events: none;
|
||||
image-rendering: pixelated;
|
||||
}
|
||||
|
||||
.map-editor {
|
||||
position: absolute;
|
||||
top: 0;
|
||||
left: 0;
|
||||
transform: scale(1, -1);
|
||||
transform-origin: center;
|
||||
}
|
||||
|
||||
.map-mouse-position {
|
||||
position: absolute;
|
||||
bottom: 10px;
|
||||
left: 10px;
|
||||
background-color: rgba(0, 0, 0, 0.7);
|
||||
color: white;
|
||||
padding: 5px 10px;
|
||||
border-radius: 4px;
|
||||
font-size: 12px;
|
||||
display: flex;
|
||||
gap: 10px;
|
||||
}
|
||||
|
||||
.map-mouse-position span {
|
||||
white-space: nowrap;
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
<div style="position: absolute; top: 5px; left: 5px; background-color: white; border-radius: 4px; color: #00cc66; font-size: 15px; font-weight: bold;">
|
||||
<div class="px-1 pt-1">
|
||||
<span class="mdi mdi-cursor-default-outline" /> @X, @Y
|
||||
</div>
|
||||
</div>
|
||||
|
||||
@code {
|
||||
private string X = "0.000";
|
||||
private string Y = "0.000";
|
||||
|
||||
public void Update(double x, double y)
|
||||
{
|
||||
X = x.ToString("N3");
|
||||
Y = y.ToString("N3");
|
||||
StateHasChanged();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
@using RobotNet10.Shared.Geometry
|
||||
@if (_visible)
|
||||
{
|
||||
<!-- Goal crosshair -->
|
||||
<circle cx="@_gx" cy="@_gy" r="0.08" fill="none" stroke="#FF4500" stroke-width="0.03" />
|
||||
<line x1="@_gxLeft" y1="@_gy" x2="@_gxRight" y2="@_gy" stroke="#FF4500" stroke-width="0.02" />
|
||||
<line x1="@_gx" y1="@_gyBottom" x2="@_gx" y2="@_gyTop" stroke="#FF4500" stroke-width="0.02" />
|
||||
<!-- Reference points -->
|
||||
@foreach (var pt in _referencePoints)
|
||||
{
|
||||
<line x1="@_gx" y1="@_gy" x2="@pt.X" y2="@pt.Y" stroke="#00FFFF" stroke-width="0.01" stroke-dasharray="0.05,0.03" />
|
||||
<circle cx="@pt.X" cy="@pt.Y" r="0.04" fill="#00FFFF" stroke="#008B8B" stroke-width="0.015" />
|
||||
}
|
||||
}
|
||||
|
||||
@code {
|
||||
private bool _visible;
|
||||
private double _gx, _gy;
|
||||
private double _gxLeft, _gxRight, _gyBottom, _gyTop;
|
||||
private List<(double X, double Y)> _referencePoints = [];
|
||||
|
||||
public void Update(Pose goal, List<(double X, double Y)> referencePoints)
|
||||
{
|
||||
_gx = goal.Position.X;
|
||||
_gy = goal.Position.Y;
|
||||
_gxLeft = _gx - 0.12;
|
||||
_gxRight = _gx + 0.12;
|
||||
_gyBottom = _gy - 0.12;
|
||||
_gyTop = _gy + 0.12;
|
||||
_referencePoints = referencePoints;
|
||||
_visible = true;
|
||||
StateHasChanged();
|
||||
}
|
||||
|
||||
public void Clear()
|
||||
{
|
||||
_visible = false;
|
||||
_referencePoints = [];
|
||||
StateHasChanged();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
@using Microsoft.AspNetCore.Components
|
||||
|
||||
<!-- Robot pose visualization component. SVG trong MapLocalization dùng viewBox (met), scaleY(-1); x, y, r đơn vị mét. -->
|
||||
<g transform="@Transform">
|
||||
<!-- Robot image (width: 1.106m, height: 0.606m) -->
|
||||
<image href="images/AS_AGV SLAM V3-CK 02.png"
|
||||
width="1.106"
|
||||
height="0.606"
|
||||
transform="translate(-0.553, -0.303)" />
|
||||
</g>
|
||||
|
||||
@code {
|
||||
private double _currentX = -1000;
|
||||
private double _currentY = -1000;
|
||||
private double _yaw = 0;
|
||||
|
||||
/// <summary>
|
||||
/// Transform string for SVG group element
|
||||
/// </summary>
|
||||
private string Transform => $"translate({_currentX:F6},{_currentY:F6}) rotate({_yaw * 180.0 / Math.PI:F2})";
|
||||
|
||||
/// <summary>
|
||||
/// Update robot pose với vị trí và góc quay
|
||||
/// </summary>
|
||||
/// <param name="x">X position trong world coordinates</param>
|
||||
/// <param name="y">Y position trong world coordinates</param>
|
||||
/// <param name="yaw">Yaw angle trong radians</param>
|
||||
public void UpdatePose(double x, double y, double yaw)
|
||||
{
|
||||
_currentX = x;
|
||||
_currentY = y;
|
||||
_yaw = yaw;
|
||||
StateHasChanged();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
<div style="position: absolute; top: 5px; right: 5px; background-color: white; border-radius: 4px; color: #00cc66; font-size: 15px; font-weight: bold;">
|
||||
<div class="px-1 pt-1">
|
||||
@X, @Y <span class="mdi mdi-compass-outline" />@Yaw - @Score
|
||||
</div>
|
||||
</div>
|
||||
|
||||
@code {
|
||||
private string X = "0.000";
|
||||
private string Y = "0.000";
|
||||
private string Yaw = "0.000";
|
||||
private string Score = "00.00%";
|
||||
|
||||
public void Update(double x, double y, double yaw, double score)
|
||||
{
|
||||
X = x.ToString("N3");
|
||||
Y = y.ToString("N3");
|
||||
Yaw = (yaw * 180.0 / Math.PI).ToString("N3"); // Convert radians to degrees
|
||||
Score = score.ToString("P1", System.Globalization.CultureInfo.InvariantCulture);
|
||||
StateHasChanged();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
@using MudBlazor
|
||||
@using RobotNet10.RobotApp.Client.Shared.SLAM
|
||||
|
||||
<MudDialog>
|
||||
<TitleContent>
|
||||
<MudText Typo="Typo.h6">Select Map for Localization</MudText>
|
||||
</TitleContent>
|
||||
<DialogContent>
|
||||
@if (Maps == null || Maps.Length == 0)
|
||||
{
|
||||
<MudText Typo="Typo.body1" Color="Color.Secondary">
|
||||
No maps available. Please create a map first.
|
||||
</MudText>
|
||||
}
|
||||
else
|
||||
{
|
||||
<MudList T="MapInfoDto">
|
||||
@foreach (var map in Maps)
|
||||
{
|
||||
<MudListItem OnClick="@(() => SelectMap(map.Name))">
|
||||
<MudText Typo="Typo.body1">@map.Name</MudText>
|
||||
<MudText Typo="Typo.caption" Color="Color.Secondary">
|
||||
Created: @map.CreatedDate.ToString("yyyy-MM-dd HH:mm:ss")
|
||||
</MudText>
|
||||
</MudListItem>
|
||||
}
|
||||
</MudList>
|
||||
}
|
||||
</DialogContent>
|
||||
<DialogActions>
|
||||
<MudButton OnClick="Cancel">Cancel</MudButton>
|
||||
</DialogActions>
|
||||
</MudDialog>
|
||||
|
||||
@code {
|
||||
[CascadingParameter] IMudDialogInstance Dialog { get; set; } = null!;
|
||||
[Parameter] public MapInfoDto[] Maps { get; set; } = Array.Empty<MapInfoDto>();
|
||||
|
||||
private void Cancel() => Dialog.Cancel();
|
||||
|
||||
private void SelectMap(string mapName)
|
||||
{
|
||||
Dialog.Close(DialogResult.Ok(mapName));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user