605 lines
20 KiB
C#
605 lines
20 KiB
C#
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
|
|
}
|