Initial commit
This commit is contained in:
@@ -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
|
||||
}
|
||||
Reference in New Issue
Block a user