2815 lines
116 KiB
JavaScript
2815 lines
116 KiB
JavaScript
// XLOC Map Renderer - Canvas-based 2D visualization for robot localization
|
|
// Renders occupancy grid maps, robot pose, and LIDAR scans
|
|
|
|
(function () {
|
|
window.robotnet = window.robotnet || {};
|
|
if (!window.robotnet.triggerFileDownload) {
|
|
window.robotnet.triggerFileDownload = function (url) {
|
|
const a = document.createElement("a");
|
|
a.href = url;
|
|
a.rel = "noopener";
|
|
document.body.appendChild(a);
|
|
a.click();
|
|
document.body.removeChild(a);
|
|
};
|
|
}
|
|
if (!window.robotnet.clickElementById) {
|
|
window.robotnet.clickElementById = function (elementId) {
|
|
const el = document.getElementById(elementId);
|
|
if (el) el.click();
|
|
};
|
|
}
|
|
})();
|
|
|
|
window.xlocMapRenderer = {
|
|
canvas: null,
|
|
ctx: null,
|
|
gridMap: null,
|
|
onlineMapOverlay: null, // Online map overlay for update map mode
|
|
mapCacheCanvas: null,
|
|
mapCacheCtx: null,
|
|
mapCacheReady: false,
|
|
globalPathData: null,
|
|
showGlobalPath: false,
|
|
localPathData: null,
|
|
showLocalPath: true,
|
|
costMapData: null,
|
|
costMapCacheCanvas: null,
|
|
costMapCacheCtx: null,
|
|
costMapCacheReady: false,
|
|
costMapPoseAnchor: null,
|
|
costMapOdomAnchor: null,
|
|
costMapFixedYaw: null,
|
|
costMapMapFromOdomLock: null,
|
|
costMapLocalizationActive: false,
|
|
costMapLastMatchingScore: -1,
|
|
showCostMap: false,
|
|
showRobotPose: true,
|
|
robotFootprint: null, // Array of {x, y} points defining robot footprint
|
|
showRobotFootprint: true,
|
|
robotPose: null,
|
|
laserScanData: null,
|
|
lidarDisplayOptions: {
|
|
lidar1: { visible: true, mode: 'minimal' },
|
|
lidar2: { visible: true, mode: 'minimal' },
|
|
lidar3: { visible: true, mode: 'minimal' }
|
|
},
|
|
cursorTooltipEl: null,
|
|
cursorStyle: "url('data:image/svg+xml;utf8,<svg xmlns=\"http://www.w3.org/2000/svg\" width=\"24\" height=\"24\" viewBox=\"0 0 24 24\"><line x1=\"0\" y1=\"12\" x2=\"24\" y2=\"12\" stroke=\"%23000000\" stroke-width=\"2\"/><line x1=\"12\" y1=\"0\" x2=\"12\" y2=\"24\" stroke=\"%23000000\" stroke-width=\"2\"/></svg>') 12 12, crosshair",
|
|
initialPoseCursor: "url('data:image/svg+xml;utf8,<svg xmlns=\"http://www.w3.org/2000/svg\" width=\"24\" height=\"24\" viewBox=\"0 0 24 24\"><line x1=\"0\" y1=\"12\" x2=\"24\" y2=\"12\" stroke=\"%23000000\" stroke-width=\"2\"/><line x1=\"12\" y1=\"0\" x2=\"12\" y2=\"24\" stroke=\"%23000000\" stroke-width=\"2\"/></svg>') 12 12, crosshair",
|
|
resizeObserver: null,
|
|
renderPending: false,
|
|
dotNetHelper: null,
|
|
|
|
// Camera/view parameters
|
|
offsetX: 0,
|
|
offsetY: 0,
|
|
scale: 20, // pixels per meter (zoom level)
|
|
|
|
// LOCKED values - NEVER change after map is loaded
|
|
lockedMapOrigin: null, // {x, y, z} - Map origin is FIXED
|
|
lockedOffsetX: null, // View offset X is FIXED after initial pose
|
|
lockedOffsetY: null, // View offset Y is FIXED after initial pose
|
|
|
|
// Drag state
|
|
isDragging: false,
|
|
dragStartX: 0,
|
|
dragStartY: 0,
|
|
lastOffsetX: 0,
|
|
lastOffsetY: 0,
|
|
|
|
// Initial pose selection state
|
|
initialPose: null,
|
|
initialPoseMode: false,
|
|
initialPoseDragging: false,
|
|
initialPoseDragBase: null,
|
|
|
|
/**
|
|
* Initialize the map renderer with a canvas element
|
|
*/
|
|
init: function(canvasElement) {
|
|
this.canvas = canvasElement;
|
|
if (!this.canvas) {
|
|
console.error('Canvas element not found');
|
|
return;
|
|
}
|
|
|
|
this.ctx = this.canvas.getContext('2d');
|
|
if (!this.ctx) {
|
|
console.error('Failed to get 2D context');
|
|
return;
|
|
}
|
|
|
|
this.cursorTooltipEl = document.getElementById('xloc-map-cursor-tooltip');
|
|
this.setupResizeObserver();
|
|
|
|
// Set canvas size to match container
|
|
this.resize();
|
|
|
|
// Setup mouse event handlers for pan/zoom
|
|
this.setupEventHandlers();
|
|
|
|
// Initial render
|
|
this.render();
|
|
},
|
|
|
|
/**
|
|
* Setup mouse event handlers for panning
|
|
*/
|
|
setupEventHandlers: function() {
|
|
if (!this.canvas) return;
|
|
|
|
// Mouse down - start dragging
|
|
this.canvas.addEventListener('mousedown', (e) => {
|
|
if (e.button !== 0) return; // Only handle left mouse button
|
|
|
|
// CRITICAL: Check initial pose mode FIRST to prevent panning conflict
|
|
if (this.initialPoseMode) {
|
|
console.log("Initial pose mode: mouse down at", e.offsetX, e.offsetY);
|
|
const mapCoords = this.getMapCoordsFromEvent(e);
|
|
console.log("Map coordinates:", mapCoords.mapX, mapCoords.mapY);
|
|
|
|
this.initialPoseDragging = true;
|
|
this.initialPoseDragBase = { x: mapCoords.mapX, y: mapCoords.mapY };
|
|
|
|
// Initialize pose at click position with default yaw (0 = pointing right/+X)
|
|
const existingYaw = this.initialPose ? this.initialPose.yaw : 0;
|
|
this.initialPose = { x: mapCoords.mapX, y: mapCoords.mapY, yaw: existingYaw };
|
|
|
|
// Update pose from current mouse position (will set yaw based on drag)
|
|
this.updateInitialPoseFromEvent(e);
|
|
this.requestRender();
|
|
|
|
e.preventDefault(); // Prevent default behavior
|
|
e.stopPropagation(); // Stop event propagation
|
|
return;
|
|
}
|
|
|
|
// Normal panning mode
|
|
this.isDragging = true;
|
|
this.dragStartX = e.offsetX;
|
|
this.dragStartY = e.offsetY;
|
|
this.lastOffsetX = this.offsetX;
|
|
this.lastOffsetY = this.offsetY;
|
|
this.canvas.style.cursor = this.cursorStyle;
|
|
});
|
|
|
|
// Mouse move - pan view
|
|
this.canvas.addEventListener('mousemove', (e) => {
|
|
this.updateCursorTooltip(e);
|
|
|
|
// CRITICAL: Check initial pose dragging FIRST to prevent panning conflict
|
|
if (this.initialPoseDragging) {
|
|
this.updateInitialPoseFromEvent(e);
|
|
this.requestRender();
|
|
e.preventDefault(); // Prevent default behavior during drag
|
|
e.stopPropagation(); // Stop event propagation
|
|
return;
|
|
}
|
|
|
|
// Normal panning mode
|
|
if (this.isDragging) {
|
|
const dx = e.offsetX - this.dragStartX;
|
|
const dy = e.offsetY - this.dragStartY;
|
|
this.offsetX = this.lastOffsetX + dx;
|
|
this.offsetY = this.lastOffsetY + dy;
|
|
|
|
// CRITICAL: Update locked offsets when user pans (user-initiated action)
|
|
if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
|
|
this.lockedOffsetX = this.offsetX;
|
|
this.lockedOffsetY = this.offsetY;
|
|
}
|
|
|
|
this.render();
|
|
}
|
|
});
|
|
|
|
// Mouse up - stop dragging
|
|
this.canvas.addEventListener('mouseup', (e) => {
|
|
if (this.initialPoseDragging) {
|
|
console.log("Initial pose drag ended");
|
|
// Update pose one final time to ensure it's current
|
|
this.updateInitialPoseFromEvent(e);
|
|
console.log("Final initial pose:", this.initialPose);
|
|
this.initialPoseDragging = false;
|
|
this.notifyInitialPoseSelected();
|
|
e.preventDefault();
|
|
e.stopPropagation();
|
|
return;
|
|
}
|
|
this.isDragging = false;
|
|
this.canvas.style.cursor = this.cursorStyle;
|
|
});
|
|
|
|
// Mouse leave - stop dragging
|
|
this.canvas.addEventListener('mouseleave', () => {
|
|
if (this.initialPoseDragging) {
|
|
this.initialPoseDragging = false;
|
|
this.notifyInitialPoseSelected();
|
|
}
|
|
this.isDragging = false;
|
|
this.canvas.style.cursor = this.cursorStyle;
|
|
this.hideCursorTooltip();
|
|
});
|
|
|
|
// Mouse wheel - zoom
|
|
this.canvas.addEventListener('wheel', (e) => {
|
|
e.preventDefault();
|
|
|
|
// Zoom factor
|
|
const zoomFactor = e.deltaY > 0 ? 0.9 : 1.1;
|
|
const newScale = this.scale * zoomFactor;
|
|
|
|
// Clamp zoom level
|
|
if (newScale >= 1 && newScale <= 100) {
|
|
// Zoom towards mouse position
|
|
const mouseX = e.offsetX - this.canvas.width / 2 - this.offsetX;
|
|
const mouseY = e.offsetY - this.canvas.height / 2 - this.offsetY;
|
|
|
|
this.scale = newScale;
|
|
|
|
// Adjust offset to zoom towards mouse
|
|
this.offsetX -= mouseX * (zoomFactor - 1);
|
|
this.offsetY -= mouseY * (zoomFactor - 1);
|
|
|
|
// CRITICAL: Update locked offsets when user zooms (user-initiated action)
|
|
if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
|
|
this.lockedOffsetX = this.offsetX;
|
|
this.lockedOffsetY = this.offsetY;
|
|
}
|
|
|
|
this.render();
|
|
}
|
|
});
|
|
|
|
// Set initial cursor
|
|
this.canvas.style.cursor = this.cursorStyle;
|
|
},
|
|
|
|
setupResizeObserver: function() {
|
|
if (!this.canvas) return;
|
|
const container = this.canvas.parentElement;
|
|
if (!container || typeof ResizeObserver === 'undefined') return;
|
|
if (this.resizeObserver) {
|
|
this.resizeObserver.disconnect();
|
|
}
|
|
this.resizeObserver = new ResizeObserver(() => {
|
|
this.resize();
|
|
});
|
|
this.resizeObserver.observe(container);
|
|
},
|
|
|
|
updateCursorTooltip: function(event) {
|
|
if (!this.canvas || !this.cursorTooltipEl) return;
|
|
const { cssX, cssY, mapX, mapY } = this.getMapCoordsFromEvent(event);
|
|
this.cursorTooltipEl.textContent = `Map: (${mapX.toFixed(3)}, ${mapY.toFixed(3)})m`;
|
|
this.cursorTooltipEl.style.left = `${cssX + 12}px`;
|
|
this.cursorTooltipEl.style.top = `${cssY + 12}px`;
|
|
this.cursorTooltipEl.style.display = 'block';
|
|
},
|
|
|
|
hideCursorTooltip: function() {
|
|
if (!this.cursorTooltipEl) return;
|
|
this.cursorTooltipEl.style.display = 'none';
|
|
},
|
|
|
|
getMapCoordsFromEvent: function(event) {
|
|
const rect = this.canvas.getBoundingClientRect();
|
|
const cssX = event.clientX - rect.left;
|
|
const cssY = event.clientY - rect.top;
|
|
const scaleX = this.canvas.width / rect.width;
|
|
const scaleY = this.canvas.height / rect.height;
|
|
const canvasX = cssX * scaleX;
|
|
const canvasY = cssY * scaleY;
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
const mapX = (canvasX - mapFrameX) / this.scale;
|
|
const mapY = (mapFrameY - canvasY) / this.scale;
|
|
return { cssX, cssY, canvasX, canvasY, mapX, mapY };
|
|
},
|
|
|
|
setDotNetHelper: function(dotNetHelper) {
|
|
this.dotNetHelper = dotNetHelper;
|
|
},
|
|
|
|
setInitialPoseMode: function(enabled) {
|
|
const wasEnabled = this.initialPoseMode;
|
|
this.initialPoseMode = !!enabled;
|
|
console.log("Initial pose mode changed:", wasEnabled ? "ON" : "OFF", "→", this.initialPoseMode ? "ON" : "OFF");
|
|
|
|
if (!this.initialPoseMode) {
|
|
// Mode disabled - stop any dragging and reset cursor
|
|
this.initialPoseDragging = false;
|
|
this.initialPoseDragBase = null;
|
|
if (this.canvas) {
|
|
this.canvas.style.cursor = this.cursorStyle;
|
|
}
|
|
console.log("Initial pose mode disabled - cursor reset");
|
|
} else {
|
|
// Mode enabled - set crosshair cursor (black)
|
|
if (this.canvas) {
|
|
this.canvas.style.cursor = this.initialPoseCursor;
|
|
console.log("Initial pose mode enabled - cursor set to crosshair");
|
|
}
|
|
// Ensure initial pose is visible when mode is enabled
|
|
// If no initial pose exists, create a default one at origin
|
|
if (!this.initialPose) {
|
|
this.initialPose = { x: 0, y: 0, yaw: 0 };
|
|
console.log("Created default initial pose at origin:", this.initialPose);
|
|
} else {
|
|
console.log("Using existing initial pose:", this.initialPose);
|
|
}
|
|
}
|
|
this.requestRender();
|
|
},
|
|
|
|
setInitialPose: function(pose) {
|
|
if (!pose) {
|
|
console.warn('setInitialPose called with null/undefined pose');
|
|
return;
|
|
}
|
|
|
|
const x = Number(pose.x ?? pose.X ?? 0);
|
|
const y = Number(pose.y ?? pose.Y ?? 0);
|
|
const yaw = Number(pose.yaw ?? pose.Yaw ?? 0);
|
|
this.initialPose = { x, y, yaw };
|
|
|
|
// CRITICAL: Use locked view offsets if available (prevents map frame from moving)
|
|
if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
|
|
// Force use locked offsets - map frame MUST stay fixed
|
|
if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 ||
|
|
Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
|
|
console.error('⚠️ CRITICAL ERROR: View offsets changed when setting initial pose!');
|
|
console.error(' Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
|
|
console.error(' Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
|
|
console.error(' → Restoring locked offsets to prevent map frame movement!');
|
|
// Restore locked offsets
|
|
this.offsetX = this.lockedOffsetX;
|
|
this.offsetY = this.lockedOffsetY;
|
|
}
|
|
}
|
|
|
|
// CRITICAL: Verify map origin has NOT changed
|
|
if (this.lockedMapOrigin) {
|
|
const currentOrigin = this.gridMap?.origin;
|
|
if (currentOrigin) {
|
|
const dx = Math.abs(currentOrigin.x - this.lockedMapOrigin.x);
|
|
const dy = Math.abs(currentOrigin.y - this.lockedMapOrigin.y);
|
|
const dz = Math.abs(currentOrigin.z - this.lockedMapOrigin.z);
|
|
|
|
if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
|
|
console.error('⚠️ CRITICAL ERROR: Map origin changed when setting initial pose!');
|
|
console.error(' Locked:', this.lockedMapOrigin);
|
|
console.error(' Current:', currentOrigin);
|
|
console.error(' → Forcing use of locked origin!');
|
|
// Force use locked origin
|
|
if (this.gridMap) {
|
|
this.gridMap.origin = {
|
|
x: this.lockedMapOrigin.x,
|
|
y: this.lockedMapOrigin.y,
|
|
z: this.lockedMapOrigin.z
|
|
};
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// IMPORTANT: Only update initial pose visualization, DO NOT reload map!
|
|
// Map origin and view offsets are locked and will not change.
|
|
this.requestRender();
|
|
},
|
|
|
|
updateInitialPoseFromEvent: function(event) {
|
|
if (!this.initialPoseDragging || !this.initialPoseDragBase) return;
|
|
const mapCoords = this.getMapCoordsFromEvent(event);
|
|
const dx = mapCoords.mapX - this.initialPoseDragBase.x;
|
|
const dy = mapCoords.mapY - this.initialPoseDragBase.y;
|
|
const distance = Math.hypot(dx, dy);
|
|
// Calculate yaw from drag direction: atan2(dy, dx) gives angle from +X axis
|
|
// If distance is too small, keep previous yaw or default to 0
|
|
const yaw = distance > 0.01 ? Math.atan2(dy, dx) : (this.initialPose ? this.initialPose.yaw : 0);
|
|
// Position stays at click point, yaw follows drag direction
|
|
this.initialPose = { x: this.initialPoseDragBase.x, y: this.initialPoseDragBase.y, yaw: yaw };
|
|
},
|
|
|
|
notifyInitialPoseSelected: function() {
|
|
if (!this.dotNetHelper || !this.initialPose) {
|
|
console.warn("Cannot notify initial pose: dotNetHelper or initialPose is null");
|
|
return;
|
|
}
|
|
console.log("Notifying initial pose selected:", {
|
|
x: this.initialPose.x,
|
|
y: this.initialPose.y,
|
|
yaw: this.initialPose.yaw,
|
|
yawDeg: this.initialPose.yaw * 180 / Math.PI
|
|
});
|
|
this.dotNetHelper.invokeMethodAsync("OnInitialPoseSelected", {
|
|
x: this.initialPose.x,
|
|
y: this.initialPose.y,
|
|
yaw: this.initialPose.yaw
|
|
}).catch((error) => {
|
|
console.error("Failed to notify initial pose selection:", error);
|
|
});
|
|
},
|
|
|
|
/**
|
|
* Get map origin x,y in meters (supports camelCase and PascalCase from C# serialization).
|
|
* Map (0,0) = World (0,0) + (originX, originY). When negative, Map is bottom-left of World.
|
|
*
|
|
* CRITICAL: Always use LOCKED origin if available to prevent map frame from moving.
|
|
*/
|
|
getMapOriginXY: function() {
|
|
// Always use locked origin if available (prevents map frame from moving)
|
|
if (this.lockedMapOrigin) {
|
|
return {
|
|
x: this.lockedMapOrigin.x,
|
|
y: this.lockedMapOrigin.y,
|
|
z: this.lockedMapOrigin.z
|
|
};
|
|
}
|
|
|
|
// Fallback to gridMap origin if locked origin not available yet
|
|
if (!this.gridMap) return { x: 0, y: 0 };
|
|
const o = this.gridMap.origin != null ? this.gridMap.origin : this.gridMap.Origin;
|
|
if (o == null) return { x: 0, y: 0 };
|
|
const x = typeof o.x === 'number' ? o.x : (typeof o.X === 'number' ? o.X : (Number(o.x) || Number(o.X) || 0));
|
|
const y = typeof o.y === 'number' ? o.y : (typeof o.Y === 'number' ? o.Y : (Number(o.y) || Number(o.Y) || 0));
|
|
return { x: x, y: y };
|
|
},
|
|
|
|
/**
|
|
* Map frame = canvas center (single origin). Robot and occupancy are computed in Map frame.
|
|
*
|
|
* Map frame position = canvas center + view offsets (for panning/zooming)
|
|
* Map origin (in world coordinates) is LOCKED and NEVER changes.
|
|
*
|
|
* CRITICAL: Always use LOCKED view offsets if available to prevent map frame from moving!
|
|
*/
|
|
getFramePositions: function() {
|
|
// CRITICAL: Use locked view offsets if available (prevents map frame from moving)
|
|
let offsetX = this.offsetX;
|
|
let offsetY = this.offsetY;
|
|
|
|
if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
|
|
// Force use locked offsets - map frame MUST stay fixed
|
|
if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 ||
|
|
Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
|
|
console.error('⚠️ CRITICAL ERROR: View offsets changed! Restoring locked offsets in getFramePositions().');
|
|
console.error(' Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
|
|
console.error(' Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
|
|
// Restore locked offsets
|
|
this.offsetX = this.lockedOffsetX;
|
|
this.offsetY = this.lockedOffsetY;
|
|
}
|
|
// Always use locked offsets for frame position calculation
|
|
offsetX = this.lockedOffsetX;
|
|
offsetY = this.lockedOffsetY;
|
|
}
|
|
|
|
const mapFrameX = this.canvas.width / 2 + offsetX;
|
|
const mapFrameY = this.canvas.height / 2 + offsetY;
|
|
return { mapFrameX, mapFrameY };
|
|
},
|
|
|
|
/**
|
|
* Set the occupancy grid map data (object - may have origin serialization issues)
|
|
*/
|
|
setGridMap: function(gridMapData) {
|
|
if (!gridMapData) return;
|
|
var o = gridMapData.origin != null ? gridMapData.origin : gridMapData.Origin;
|
|
if (gridMapData.dataBase64 && o == null) {
|
|
console.warn('xlocMapRenderer: setGridMap received data but no origin - map will render at World frame');
|
|
}
|
|
this.gridMap = gridMapData;
|
|
this.buildMapCache();
|
|
this.requestRender();
|
|
},
|
|
|
|
/**
|
|
* Set grid map with explicit origin (avoids C#->JS object serialization issues).
|
|
* Call this from C# with numeric originX, originY so map renders at Map frame.
|
|
*
|
|
* CRITICAL: Map origin is LOCKED on first call and NEVER changes!
|
|
* This ensures that when robot pose changes, the map frame itself stays fixed.
|
|
*/
|
|
setGridMapWithOrigin: function(width, height, resolution, originX, originY, originZ, dataBase64, isMapping) {
|
|
if (width == null || height == null || !dataBase64) {
|
|
console.warn('[setGridMapWithOrigin] Invalid parameters', { width, height, hasData: !!dataBase64, dataLength: dataBase64 ? dataBase64.length : 0 });
|
|
return;
|
|
}
|
|
|
|
// Only log when map changes significantly
|
|
const isNewMap = this.lockedMapOrigin === null ||
|
|
this.gridMap === null ||
|
|
this.gridMap.width !== width ||
|
|
this.gridMap.height !== height;
|
|
|
|
if (isNewMap) {
|
|
console.log(`[setGridMapWithOrigin] Setting map - Width: ${width}, Height: ${height}, Resolution: ${resolution}, Origin: (${originX}, ${originY}, ${originZ}), Data length: ${dataBase64.length}`);
|
|
}
|
|
|
|
const newOrigin = {
|
|
x: Number(originX),
|
|
y: Number(originY),
|
|
z: Number(originZ)
|
|
};
|
|
|
|
// When mapping, allow origin to update from online map to ensure alignment with LIDAR
|
|
// When not mapping, lock origin to prevent map frame movement
|
|
const isMappingMode = isMapping === true; // Check if currently mapping
|
|
|
|
if (this.lockedMapOrigin === null) {
|
|
// First time: lock origin
|
|
this.lockedMapOrigin = newOrigin;
|
|
console.log(`[setGridMapWithOrigin] Locked map origin at (${newOrigin.x}, ${newOrigin.y}, ${newOrigin.z})`);
|
|
// CRITICAL: Also lock view offsets when map is first loaded
|
|
// This ensures map frame position stays fixed
|
|
if (this.lockedOffsetX === null && this.lockedOffsetY === null) {
|
|
this.lockedOffsetX = this.offsetX;
|
|
this.lockedOffsetY = this.offsetY;
|
|
}
|
|
} else {
|
|
// Check if origin changed
|
|
const dx = Math.abs(newOrigin.x - this.lockedMapOrigin.x);
|
|
const dy = Math.abs(newOrigin.y - this.lockedMapOrigin.y);
|
|
const dz = Math.abs(newOrigin.z - this.lockedMapOrigin.z);
|
|
|
|
if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
|
|
// Origin changed - during mapping, use new origin to align with LIDAR
|
|
// During localization, keep locked origin to prevent map frame movement
|
|
if (isMappingMode) {
|
|
console.log(`[setGridMapWithOrigin] Origin updated during mapping: (${newOrigin.x.toFixed(3)}, ${newOrigin.y.toFixed(3)}, ${newOrigin.z.toFixed(3)})`);
|
|
console.log(`[setGridMapWithOrigin] Previous: (${this.lockedMapOrigin.x.toFixed(3)}, ${this.lockedMapOrigin.y.toFixed(3)}, ${this.lockedMapOrigin.z.toFixed(3)})`);
|
|
// Update locked origin to match online map (ensures alignment with LIDAR)
|
|
this.lockedMapOrigin = newOrigin;
|
|
} else {
|
|
console.warn('[setGridMapWithOrigin] Origin changed but not mapping - keeping locked origin');
|
|
// Use locked origin
|
|
originX = this.lockedMapOrigin.x;
|
|
originY = this.lockedMapOrigin.y;
|
|
originZ = this.lockedMapOrigin.z;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Use current origin (may be updated during mapping)
|
|
this.gridMap = {
|
|
width: width,
|
|
height: height,
|
|
resolution: resolution,
|
|
origin: { x: this.lockedMapOrigin.x, y: this.lockedMapOrigin.y, z: this.lockedMapOrigin.z },
|
|
dataBase64: dataBase64
|
|
};
|
|
|
|
if (isNewMap) {
|
|
console.log('[setGridMapWithOrigin] Building map cache...');
|
|
}
|
|
this.buildMapCache();
|
|
if (isNewMap) {
|
|
console.log('[setGridMapWithOrigin] Requesting render...');
|
|
}
|
|
this.requestRender();
|
|
if (isNewMap) {
|
|
console.log('[setGridMapWithOrigin] Map set and render requested');
|
|
}
|
|
},
|
|
setGridMapWithMerge: function(width, height, resolution, originX, originY, originZ, dataBase64) {
|
|
if (width == null || height == null || !dataBase64) {
|
|
console.warn('[setGridMapWithMerge] Invalid parameters', { width, height, hasData: !!dataBase64 });
|
|
return;
|
|
}
|
|
|
|
// If no existing map, just set it normally
|
|
if (!this.gridMap || !this.gridMap.dataBase64) {
|
|
console.log('[setGridMapWithMerge] No existing map, setting new map');
|
|
this.setGridMapWithOrigin(width, height, resolution, originX, originY, originZ, dataBase64, false);
|
|
return;
|
|
}
|
|
|
|
const newOrigin = {
|
|
x: Number(originX),
|
|
y: Number(originY),
|
|
z: Number(originZ)
|
|
};
|
|
|
|
// Use locked origin (don't allow origin to change during update map)
|
|
const originToUse = this.lockedMapOrigin || newOrigin;
|
|
|
|
// Decode existing map data
|
|
const existingBytes = Uint8Array.from(atob(this.gridMap.dataBase64), c => c.charCodeAt(0));
|
|
const existingWidth = this.gridMap.width;
|
|
const existingHeight = this.gridMap.height;
|
|
const existingResolution = this.gridMap.resolution;
|
|
|
|
// Decode new online map data
|
|
const newBytes = Uint8Array.from(atob(dataBase64), c => c.charCodeAt(0));
|
|
|
|
// Calculate bounds for merged map
|
|
// Both maps should use the same origin, so we need to calculate pixel offsets
|
|
const resolutionToUse = resolution;
|
|
const existingOrigin = this.gridMap.origin || originToUse;
|
|
const existingOffsetX = (existingOrigin.x - originToUse.x) / resolutionToUse;
|
|
const existingOffsetY = (existingOrigin.y - originToUse.y) / resolutionToUse;
|
|
const newOffsetX = 0; // New map uses the same origin
|
|
const newOffsetY = 0;
|
|
|
|
// Calculate bounds
|
|
const existingMinX = Math.round(existingOffsetX);
|
|
const existingMinY = Math.round(existingOffsetY);
|
|
const existingMaxX = existingMinX + existingWidth;
|
|
const existingMaxY = existingMinY + existingHeight;
|
|
const newMinX = Math.round(newOffsetX);
|
|
const newMinY = Math.round(newOffsetY);
|
|
const newMaxX = newMinX + width;
|
|
const newMaxY = newMinY + height;
|
|
|
|
const mergedMinX = Math.min(existingMinX, newMinX);
|
|
const mergedMinY = Math.min(existingMinY, newMinY);
|
|
const mergedMaxX = Math.max(existingMaxX, newMaxX);
|
|
const mergedMaxY = Math.max(existingMaxY, newMaxY);
|
|
|
|
const mergedWidth = mergedMaxX - mergedMinX;
|
|
const mergedHeight = mergedMaxY - mergedMinY;
|
|
|
|
// Create merged map data
|
|
const mergedBytes = new Uint8Array(mergedWidth * mergedHeight);
|
|
// Initialize with unknown (255)
|
|
mergedBytes.fill(255);
|
|
|
|
// Copy existing map data
|
|
for (let y = 0; y < existingHeight; y++) {
|
|
for (let x = 0; x < existingWidth; x++) {
|
|
const existingIdx = y * existingWidth + x;
|
|
const mergedX = x + existingMinX - mergedMinX;
|
|
const mergedY = y + existingMinY - mergedMinY;
|
|
|
|
if (mergedX >= 0 && mergedX < mergedWidth && mergedY >= 0 && mergedY < mergedHeight) {
|
|
const mergedIdx = mergedY * mergedWidth + mergedX;
|
|
const existingValue = existingBytes[existingIdx];
|
|
// Copy existing value (including unknown = 255)
|
|
mergedBytes[mergedIdx] = existingValue;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Merge new online map data (overwrite unknown areas and add new data)
|
|
for (let y = 0; y < height; y++) {
|
|
for (let x = 0; x < width; x++) {
|
|
const newIdx = y * width + x;
|
|
const mergedX = x + newMinX - mergedMinX;
|
|
const mergedY = y + newMinY - mergedMinY;
|
|
|
|
if (mergedX >= 0 && mergedX < mergedWidth && mergedY >= 0 && mergedY < mergedHeight) {
|
|
const mergedIdx = mergedY * mergedWidth + mergedX;
|
|
const newValue = newBytes[newIdx];
|
|
// Merge: use new value if it's not unknown, otherwise keep existing
|
|
if (newValue !== 255) {
|
|
mergedBytes[mergedIdx] = newValue;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Convert merged bytes to base64
|
|
let mergedBase64 = '';
|
|
for (let i = 0; i < mergedBytes.length; i++) {
|
|
mergedBase64 += String.fromCharCode(mergedBytes[i]);
|
|
}
|
|
mergedBase64 = btoa(mergedBase64);
|
|
|
|
// Update grid map with merged data
|
|
this.gridMap = {
|
|
width: mergedWidth,
|
|
height: mergedHeight,
|
|
resolution: resolution, // Use new map resolution (should be same)
|
|
origin: originToUse,
|
|
dataBase64: mergedBase64
|
|
};
|
|
|
|
console.log(`[setGridMapWithMerge] Merged map - Width: ${mergedWidth}, Height: ${mergedHeight}, Resolution: ${resolution}`);
|
|
this.buildMapCache();
|
|
this.requestRender();
|
|
},
|
|
/**
|
|
* Set grid map with overlay: static map as base, online map as overlay (for update map mode)
|
|
*/
|
|
setGridMapWithOverlay: function(staticWidth, staticHeight, staticResolution, originX, originY, originZ, staticDataBase64, onlineWidth, onlineHeight, onlineResolution, onlineDataBase64) {
|
|
if (staticWidth == null || staticHeight == null || !staticDataBase64 || onlineWidth == null || onlineHeight == null || !onlineDataBase64) {
|
|
console.warn('[setGridMapWithOverlay] Invalid parameters');
|
|
return;
|
|
}
|
|
|
|
const newOrigin = {
|
|
x: Number(originX),
|
|
y: Number(originY),
|
|
z: Number(originZ)
|
|
};
|
|
|
|
// Lock origin if not already locked
|
|
if (this.lockedMapOrigin === null) {
|
|
this.lockedMapOrigin = newOrigin;
|
|
console.log(`[setGridMapWithOverlay] Locked map origin at (${newOrigin.x}, ${newOrigin.y}, ${newOrigin.z})`);
|
|
if (this.lockedOffsetX === null && this.lockedOffsetY === null) {
|
|
this.lockedOffsetX = this.offsetX;
|
|
this.lockedOffsetY = this.offsetY;
|
|
}
|
|
}
|
|
|
|
// Decode static map data
|
|
const staticBytes = Uint8Array.from(atob(staticDataBase64), c => c.charCodeAt(0));
|
|
|
|
// Decode online map data
|
|
const onlineBytes = Uint8Array.from(atob(onlineDataBase64), c => c.charCodeAt(0));
|
|
|
|
// Create static map object
|
|
const staticMap = {
|
|
width: staticWidth,
|
|
height: staticHeight,
|
|
resolution: staticResolution,
|
|
origin: newOrigin,
|
|
data: staticBytes
|
|
};
|
|
|
|
// Create online map object (overlay)
|
|
const onlineMap = {
|
|
width: onlineWidth,
|
|
height: onlineHeight,
|
|
resolution: onlineResolution,
|
|
origin: newOrigin,
|
|
data: onlineBytes
|
|
};
|
|
|
|
// Store both maps
|
|
this.gridMap = staticMap;
|
|
this.onlineMapOverlay = onlineMap; // Store online map as overlay
|
|
|
|
// Build map cache with overlay
|
|
this.buildMapCacheWithOverlay();
|
|
|
|
this.requestRender();
|
|
},
|
|
|
|
/**
|
|
* Build map cache with overlay: static map as base, online map overlaid on top
|
|
*/
|
|
buildMapCacheWithOverlay: function() {
|
|
if (!this.mapCacheCanvas || !this.gridMap) return;
|
|
|
|
try {
|
|
const staticMap = this.gridMap;
|
|
const onlineMap = this.onlineMapOverlay;
|
|
|
|
if (!onlineMap) {
|
|
// Fallback to regular map cache if no overlay
|
|
this.buildMapCache();
|
|
return;
|
|
}
|
|
|
|
// Use the larger dimensions
|
|
const maxWidth = Math.max(staticMap.width, onlineMap.width);
|
|
const maxHeight = Math.max(staticMap.height, onlineMap.height);
|
|
|
|
this.mapCacheCanvas.width = maxWidth;
|
|
this.mapCacheCanvas.height = maxHeight;
|
|
|
|
const imageData = this.mapCacheCtx.createImageData(maxWidth, maxHeight);
|
|
const pixels = imageData.data;
|
|
|
|
// First, render static map as base
|
|
for (let y = 0; y < staticMap.height; y++) {
|
|
for (let x = 0; x < staticMap.width; x++) {
|
|
const idx = y * staticMap.width + x;
|
|
const byte = staticMap.data[idx];
|
|
const pixelIdx = (y * maxWidth + x) * 4;
|
|
|
|
// Convert occupancy value to color
|
|
if (byte === 0) {
|
|
// Free space - white
|
|
pixels[pixelIdx] = 255;
|
|
pixels[pixelIdx + 1] = 255;
|
|
pixels[pixelIdx + 2] = 255;
|
|
pixels[pixelIdx + 3] = 255;
|
|
} else if (byte === 100) {
|
|
// Occupied - black
|
|
pixels[pixelIdx] = 0;
|
|
pixels[pixelIdx + 1] = 0;
|
|
pixels[pixelIdx + 2] = 0;
|
|
pixels[pixelIdx + 3] = 255;
|
|
} else {
|
|
// Unknown - gray
|
|
pixels[pixelIdx] = 128;
|
|
pixels[pixelIdx + 1] = 128;
|
|
pixels[pixelIdx + 2] = 128;
|
|
pixels[pixelIdx + 3] = 255;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Then, overlay online map on top (only where online map has data)
|
|
for (let y = 0; y < onlineMap.height; y++) {
|
|
for (let x = 0; x < onlineMap.width; x++) {
|
|
const idx = y * onlineMap.width + x;
|
|
const byte = onlineMap.data[idx];
|
|
|
|
// Only overlay if online map has valid data (not unknown)
|
|
if (byte !== 205) { // 205 = unknown in occupancy grid
|
|
const pixelIdx = (y * maxWidth + x) * 4;
|
|
|
|
// Overlay online map data (with slight transparency to show both)
|
|
if (byte === 0) {
|
|
// Free space - light blue tint
|
|
pixels[pixelIdx] = Math.min(255, pixels[pixelIdx] * 0.7 + 200 * 0.3);
|
|
pixels[pixelIdx + 1] = Math.min(255, pixels[pixelIdx + 1] * 0.7 + 220 * 0.3);
|
|
pixels[pixelIdx + 2] = Math.min(255, pixels[pixelIdx + 2] * 0.7 + 255 * 0.3);
|
|
} else if (byte === 100) {
|
|
// Occupied - red tint
|
|
pixels[pixelIdx] = Math.min(255, pixels[pixelIdx] * 0.5 + 255 * 0.5);
|
|
pixels[pixelIdx + 1] = Math.min(255, pixels[pixelIdx + 1] * 0.5 + 0 * 0.5);
|
|
pixels[pixelIdx + 2] = Math.min(255, pixels[pixelIdx + 2] * 0.5 + 0 * 0.5);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
this.mapCacheCtx.putImageData(imageData, 0, 0);
|
|
this.mapCacheReady = true;
|
|
console.log('[buildMapCacheWithOverlay] Map cache with overlay built successfully');
|
|
} catch (error) {
|
|
console.error('[buildMapCacheWithOverlay] Error building map cache with overlay:', error);
|
|
this.mapCacheReady = false;
|
|
}
|
|
},
|
|
|
|
/**
|
|
* Set the robot pose
|
|
* CRITICAL: This should NEVER change view offsets or map origin!
|
|
* Only robot pose visualization changes - map frame stays FIXED!
|
|
*/
|
|
setRobotPose: function(poseData) {
|
|
// CRITICAL: Use locked view offsets if available (prevents map frame from moving)
|
|
if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
|
|
// Force use locked offsets - map frame MUST stay fixed
|
|
if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 ||
|
|
Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
|
|
console.error('⚠️ CRITICAL ERROR: View offsets changed! Restoring locked offsets.');
|
|
console.error(' Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
|
|
console.error(' Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
|
|
this.offsetX = this.lockedOffsetX;
|
|
this.offsetY = this.lockedOffsetY;
|
|
} else {
|
|
// Even if they match, force use locked offsets to be absolutely sure
|
|
this.offsetX = this.lockedOffsetX;
|
|
this.offsetY = this.lockedOffsetY;
|
|
}
|
|
}
|
|
|
|
// Update robot pose (ONLY this changes - map frame stays fixed)
|
|
this.robotPose = poseData;
|
|
|
|
// CRITICAL: Verify map origin has NOT changed
|
|
if (this.lockedMapOrigin && this.gridMap?.origin) {
|
|
const dx = Math.abs(this.gridMap.origin.x - this.lockedMapOrigin.x);
|
|
const dy = Math.abs(this.gridMap.origin.y - this.lockedMapOrigin.y);
|
|
const dz = Math.abs(this.gridMap.origin.z - this.lockedMapOrigin.z);
|
|
|
|
if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
|
|
console.error('⚠️ CRITICAL ERROR: Map origin changed when setting robot pose!');
|
|
console.error(' Locked:', this.lockedMapOrigin);
|
|
console.error(' Current:', this.gridMap.origin);
|
|
console.error(' → Forcing use of locked origin!');
|
|
// Force use locked origin
|
|
this.gridMap.origin = {
|
|
x: this.lockedMapOrigin.x,
|
|
y: this.lockedMapOrigin.y,
|
|
z: this.lockedMapOrigin.z
|
|
};
|
|
}
|
|
}
|
|
|
|
this.requestRender();
|
|
},
|
|
|
|
/**
|
|
* Set laser scan data for all LIDARs
|
|
*/
|
|
setLaserScanData: function(allLidarData) {
|
|
this.laserScanData = allLidarData;
|
|
this.requestRender();
|
|
},
|
|
|
|
/**
|
|
* Batch update pose + laser in one call to reduce round-trips and render once (smoother updates).
|
|
*/
|
|
updatePoseAndLaser: function(poseData, allLidarData) {
|
|
if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
|
|
if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 ||
|
|
Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
|
|
this.offsetX = this.lockedOffsetX;
|
|
this.offsetY = this.lockedOffsetY;
|
|
}
|
|
}
|
|
if (poseData) this.robotPose = poseData;
|
|
if (allLidarData) this.laserScanData = allLidarData;
|
|
if (this.lockedMapOrigin && this.gridMap && this.gridMap.origin) {
|
|
const dx = Math.abs(this.gridMap.origin.x - this.lockedMapOrigin.x);
|
|
const dy = Math.abs(this.gridMap.origin.y - this.lockedMapOrigin.y);
|
|
const dz = Math.abs(this.gridMap.origin.z - this.lockedMapOrigin.z);
|
|
if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
|
|
this.gridMap.origin = { x: this.lockedMapOrigin.x, y: this.lockedMapOrigin.y, z: this.lockedMapOrigin.z };
|
|
}
|
|
}
|
|
this.requestRender();
|
|
},
|
|
|
|
setLidarDisplayOptions: function(options) {
|
|
if (!options || typeof options !== 'object') {
|
|
return;
|
|
}
|
|
|
|
const normalizeMode = (mode) => (mode === 'full' ? 'full' : 'minimal');
|
|
const normalizeOne = (source, fallback) => ({
|
|
visible: source && typeof source.visible === 'boolean' ? source.visible : fallback.visible,
|
|
mode: normalizeMode(source && source.mode)
|
|
});
|
|
|
|
this.lidarDisplayOptions = {
|
|
lidar1: normalizeOne(options.lidar1, this.lidarDisplayOptions.lidar1),
|
|
lidar2: normalizeOne(options.lidar2, this.lidarDisplayOptions.lidar2),
|
|
lidar3: normalizeOne(options.lidar3, this.lidarDisplayOptions.lidar3)
|
|
};
|
|
|
|
this.requestRender();
|
|
},
|
|
|
|
setGlobalPathVisible: function(visible) {
|
|
this.showGlobalPath = !!visible;
|
|
console.log(`🔄 Global Path visibility: ${this.showGlobalPath ? 'ENABLED' : 'DISABLED'}`);
|
|
this.requestRender();
|
|
},
|
|
|
|
/**
|
|
* Explicitly override the locked map origin (used when the user changes
|
|
* the map origin via the Change Map Origin UI). This bypasses the
|
|
* "locked origin" guard so that the next map reload can apply the new origin.
|
|
*/
|
|
setMapOrigin: function(originX, originY, originZ) {
|
|
const newOrigin = {
|
|
x: Number(originX),
|
|
y: Number(originY),
|
|
z: Number(originZ)
|
|
};
|
|
if (!Number.isFinite(newOrigin.x) || !Number.isFinite(newOrigin.y) || !Number.isFinite(newOrigin.z)) {
|
|
console.warn('[setMapOrigin] Invalid origin values, ignored', newOrigin);
|
|
return;
|
|
}
|
|
console.log(`[setMapOrigin] Updating locked origin -> (${newOrigin.x.toFixed(3)}, ${newOrigin.y.toFixed(3)}, ${newOrigin.z.toFixed(3)})`);
|
|
this.lockedMapOrigin = newOrigin;
|
|
if (this.gridMap) {
|
|
this.gridMap.origin = { x: newOrigin.x, y: newOrigin.y, z: newOrigin.z };
|
|
}
|
|
this.requestRender();
|
|
},
|
|
|
|
/**
|
|
* Clear the locked origin so the next setGridMapWithOrigin call re-locks it.
|
|
*/
|
|
clearMapOriginLock: function() {
|
|
console.log('[clearMapOriginLock] Locked origin cleared; next map load will relock.');
|
|
this.lockedMapOrigin = null;
|
|
},
|
|
|
|
setLocalPathVisible: function(visible) {
|
|
this.showLocalPath = !!visible;
|
|
console.log(`🔄 Local Path visibility: ${this.showLocalPath ? 'ENABLED' : 'DISABLED'}`);
|
|
this.requestRender();
|
|
},
|
|
|
|
setGlobalPathDataJson: function(jsonString) {
|
|
console.log('🔍 setGlobalPathDataJson called with JSON string of length:', jsonString ? jsonString.length : 'null');
|
|
|
|
if (!jsonString) {
|
|
console.warn('⚠️ Global Path: jsonString is null/undefined');
|
|
this.globalPathData = [];
|
|
this.requestRender();
|
|
return;
|
|
}
|
|
|
|
try {
|
|
const pathPoints = JSON.parse(jsonString);
|
|
console.log('✅ JSON parsed successfully:', pathPoints);
|
|
this.setGlobalPathData(pathPoints);
|
|
} catch (error) {
|
|
console.error('❌ Error parsing JSON:', error);
|
|
console.error(' Raw string:', jsonString);
|
|
this.globalPathData = [];
|
|
this.requestRender();
|
|
}
|
|
},
|
|
|
|
setLocalPathDataJson: function(jsonString) {
|
|
if (!jsonString) {
|
|
this.localPathData = [];
|
|
this.requestRender();
|
|
return;
|
|
}
|
|
|
|
try {
|
|
const pathPoints = JSON.parse(jsonString);
|
|
this.setLocalPathData(pathPoints);
|
|
} catch (error) {
|
|
console.error('❌ Local Path: Error parsing JSON:', error);
|
|
this.localPathData = [];
|
|
this.requestRender();
|
|
}
|
|
},
|
|
|
|
setGlobalPathData: function(pathPoints) {
|
|
console.log('\ud83d\udd0d setGlobalPathData called with:', pathPoints);
|
|
console.log(' Type:', typeof pathPoints);
|
|
console.log(' Is Array:', Array.isArray(pathPoints));
|
|
console.log(' Length:', pathPoints ? pathPoints.length : 'N/A');
|
|
|
|
if (!pathPoints) {
|
|
console.warn('\u274c Global Path: pathPoints is null/undefined');
|
|
this.globalPathData = [];
|
|
this.requestRender();
|
|
return;
|
|
}
|
|
|
|
if (!Array.isArray(pathPoints)) {
|
|
console.warn('\u274c Global Path: pathPoints is not an array, type:', typeof pathPoints);
|
|
console.warn(' Value:', pathPoints);
|
|
this.globalPathData = [];
|
|
this.requestRender();
|
|
return;
|
|
}
|
|
|
|
if (pathPoints.length === 0) {
|
|
console.warn('\u26a0\ufe0f Global Path: Received empty array (0 points)');
|
|
this.globalPathData = [];
|
|
this.requestRender();
|
|
return;
|
|
}
|
|
|
|
console.log(`\ud83d\udd0d Raw data sample (first 3 points):`);
|
|
for (let i = 0; i < Math.min(3, pathPoints.length); i++) {
|
|
console.log(` Point ${i}:`, pathPoints[i]);
|
|
}
|
|
|
|
const mappedPoints = pathPoints.map((point, idx) => {
|
|
const mapped = {
|
|
x: Number(point?.x ?? point?.X ?? 0),
|
|
y: Number(point?.y ?? point?.Y ?? 0),
|
|
theta: Number(point?.theta ?? point?.Theta ?? 0)
|
|
};
|
|
if (idx < 3) {
|
|
console.log(` Mapped ${idx}:`, mapped, '(finite:', Number.isFinite(mapped.x) && Number.isFinite(mapped.y), ')');
|
|
}
|
|
return mapped;
|
|
});
|
|
|
|
this.globalPathData = mappedPoints.filter(point => Number.isFinite(point.x) && Number.isFinite(point.y));
|
|
|
|
console.log(`\u2705 Global Path: Processed ${this.globalPathData.length}/${pathPoints.length} valid points`);
|
|
if (this.globalPathData.length > 0) {
|
|
const first = this.globalPathData[0];
|
|
const last = this.globalPathData[this.globalPathData.length - 1];
|
|
console.log(` First point: (${first.x.toFixed(2)}, ${first.y.toFixed(2)})`);
|
|
console.log(` Last point: (${last.x.toFixed(2)}, ${last.y.toFixed(2)})`);
|
|
} else {
|
|
console.warn('\u26a0\ufe0f All points were filtered out! Check if coordinates are valid.');
|
|
}
|
|
|
|
this.requestRender();
|
|
},
|
|
|
|
setLocalPathData: function(pathPoints) {
|
|
if (!Array.isArray(pathPoints) || pathPoints.length === 0) {
|
|
this.localPathData = [];
|
|
this.requestRender();
|
|
return;
|
|
}
|
|
|
|
const mappedPoints = pathPoints.map((point) => ({
|
|
x: Number(point?.x ?? point?.X ?? 0),
|
|
y: Number(point?.y ?? point?.Y ?? 0),
|
|
theta: Number(point?.theta ?? point?.Theta ?? 0)
|
|
}));
|
|
|
|
this.localPathData = mappedPoints.filter(point => Number.isFinite(point.x) && Number.isFinite(point.y));
|
|
this.requestRender();
|
|
},
|
|
|
|
/**
|
|
* Debug helper: Get current path rendering status
|
|
*/
|
|
getPathDebugInfo: function() {
|
|
const info = {
|
|
showGlobalPath: this.showGlobalPath,
|
|
pathDataExists: !!this.globalPathData,
|
|
pathPointCount: this.globalPathData ? this.globalPathData.length : 0,
|
|
scale: this.scale,
|
|
offsetX: this.offsetX,
|
|
offsetY: this.offsetY,
|
|
canvasSize: this.canvas ? `${this.canvas.width}x${this.canvas.height}` : 'no canvas'
|
|
};
|
|
|
|
if (this.globalPathData && this.globalPathData.length > 0) {
|
|
info.firstPoint = this.globalPathData[0];
|
|
info.lastPoint = this.globalPathData[this.globalPathData.length - 1];
|
|
}
|
|
|
|
console.log('🔍 Path Debug Info:', info);
|
|
return info;
|
|
},
|
|
|
|
/**
|
|
* Debug helper: Force path render with test data
|
|
*/
|
|
testPathRender: function() {
|
|
console.log('🧪 Testing path render with sample data...');
|
|
|
|
// Use current robot position as starting point if available
|
|
const startX = this.robotPose ? this.robotPose.x : 0;
|
|
const startY = this.robotPose ? this.robotPose.y : 0;
|
|
|
|
// Create a simple test path (square pattern)
|
|
const testPath = [
|
|
{ x: startX, y: startY, theta: 0 },
|
|
{ x: startX + 1, y: startY, theta: 0 },
|
|
{ x: startX + 1, y: startY + 1, theta: 1.57 },
|
|
{ x: startX, y: startY + 1, theta: 3.14 },
|
|
{ x: startX, y: startY, theta: -1.57 }
|
|
];
|
|
|
|
this.showGlobalPath = true;
|
|
this.setGlobalPathData(testPath);
|
|
console.log('✅ Test path set. Should see green square path on canvas.');
|
|
},
|
|
|
|
/**
|
|
* Resize canvas to match container
|
|
*/
|
|
resize: function() {
|
|
if (!this.canvas) return;
|
|
|
|
const container = this.canvas.parentElement;
|
|
if (container) {
|
|
this.canvas.width = container.clientWidth;
|
|
this.canvas.height = container.clientHeight;
|
|
this.requestRender();
|
|
}
|
|
},
|
|
|
|
requestRender: function() {
|
|
if (this.renderPending) return;
|
|
this.renderPending = true;
|
|
requestAnimationFrame(() => {
|
|
this.renderPending = false;
|
|
this.render();
|
|
});
|
|
},
|
|
|
|
/**
|
|
* Force immediate render (for debugging)
|
|
*/
|
|
forceRender: function() {
|
|
this.renderPending = false;
|
|
this.render();
|
|
},
|
|
|
|
buildMapCache: function() {
|
|
if (!this.gridMap || !this.gridMap.dataBase64) {
|
|
console.warn('[buildMapCache] No grid map data or dataBase64 missing');
|
|
this.mapCacheReady = false;
|
|
return;
|
|
}
|
|
|
|
try {
|
|
// Decode base64 data once and cache as a canvas
|
|
const binaryString = atob(this.gridMap.dataBase64);
|
|
const bytes = new Uint8Array(binaryString.length);
|
|
for (let i = 0; i < binaryString.length; i++) {
|
|
bytes[i] = binaryString.charCodeAt(i);
|
|
}
|
|
|
|
const width = this.gridMap.width;
|
|
const height = this.gridMap.height;
|
|
|
|
// Only log first time or when significant changes
|
|
const shouldLog = !this.mapCacheReady || (width !== this.mapCacheCanvas.width || height !== this.mapCacheCanvas.height);
|
|
if (shouldLog) {
|
|
console.log(`[buildMapCache] Building map cache - Width: ${width}, Height: ${height}, Data length: ${bytes.length}, Resolution: ${this.gridMap.resolution}`);
|
|
}
|
|
|
|
if (!this.mapCacheCanvas) {
|
|
this.mapCacheCanvas = document.createElement('canvas');
|
|
this.mapCacheCtx = this.mapCacheCanvas.getContext('2d');
|
|
}
|
|
|
|
this.mapCacheCanvas.width = width;
|
|
this.mapCacheCanvas.height = height;
|
|
|
|
const imageData = this.mapCacheCtx.createImageData(width, height);
|
|
|
|
// Count occupancy values for debugging (only when logging)
|
|
let occupiedCount = 0;
|
|
let freeCount = 0;
|
|
let unknownCount = 0;
|
|
|
|
// Optimize: process in batches for better performance
|
|
for (let i = 0; i < bytes.length; i++) {
|
|
// Occupancy values: int8_t from C API
|
|
// -1 (0xFF as unsigned) = Unknown
|
|
// 0 = Free
|
|
// 1-99 = Probability of occupancy (higher = more likely occupied)
|
|
// 100 = Occupied
|
|
// When read as unsigned byte: -1 becomes 255
|
|
const occupancy = bytes[i];
|
|
let r, g, b, a = 0;
|
|
|
|
// Render occupied cells: 100 (definitely occupied) or >= 50 (likely occupied)
|
|
// This ensures we see boundaries even if probability is not exactly 100
|
|
if (occupancy === 100 || (occupancy >= 50 && occupancy < 100)) {
|
|
r = g = b = 0; // Black
|
|
a = 255; // Fully opaque
|
|
if (shouldLog) {
|
|
if (occupancy === 100) {
|
|
occupiedCount++;
|
|
} else {
|
|
occupiedCount++; // Count probability cells as occupied too
|
|
}
|
|
}
|
|
} else {
|
|
r = g = b = 0;
|
|
a = 0; // Transparent
|
|
if (shouldLog) {
|
|
if (occupancy === 0) {
|
|
freeCount++;
|
|
} else if (occupancy === 255) {
|
|
unknownCount++;
|
|
} else {
|
|
unknownCount++; // Other values treated as unknown
|
|
}
|
|
}
|
|
}
|
|
|
|
const idx = i * 4;
|
|
imageData.data[idx] = r;
|
|
imageData.data[idx + 1] = g;
|
|
imageData.data[idx + 2] = b;
|
|
imageData.data[idx + 3] = a;
|
|
}
|
|
|
|
if (shouldLog) {
|
|
console.log(`[buildMapCache] Occupancy stats - Occupied: ${occupiedCount}, Free: ${freeCount}, Unknown: ${unknownCount}`);
|
|
}
|
|
|
|
this.mapCacheCtx.putImageData(imageData, 0, 0);
|
|
this.mapCacheReady = true;
|
|
console.log('[buildMapCache] Map cache built successfully');
|
|
} catch (error) {
|
|
console.error('[buildMapCache] Error building map cache:', error);
|
|
this.mapCacheReady = false;
|
|
}
|
|
},
|
|
|
|
/**
|
|
* Main render function - draws everything
|
|
*
|
|
* CRITICAL: This function should NEVER change map origin or view offsets.
|
|
* Map frame position is calculated from locked origin + current view offsets.
|
|
*/
|
|
render: function() {
|
|
if (!this.ctx || !this.canvas) return;
|
|
|
|
// CRITICAL: Use locked view offsets if available (prevents map frame from moving)
|
|
if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
|
|
// Force use locked offsets - map frame MUST stay fixed
|
|
if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 ||
|
|
Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
|
|
console.error('⚠️ CRITICAL ERROR: View offsets changed at start of render! Restoring locked offsets.');
|
|
console.error(' Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
|
|
console.error(' Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
|
|
this.offsetX = this.lockedOffsetX;
|
|
this.offsetY = this.lockedOffsetY;
|
|
} else {
|
|
// Even if they match, force use locked offsets to be absolutely sure
|
|
this.offsetX = this.lockedOffsetX;
|
|
this.offsetY = this.lockedOffsetY;
|
|
}
|
|
}
|
|
|
|
// Save view state at start of render
|
|
const renderStartOffsetX = this.offsetX;
|
|
const renderStartOffsetY = this.offsetY;
|
|
const renderStartScale = this.scale;
|
|
|
|
// Verify map origin hasn't changed during render
|
|
if (this.lockedMapOrigin && this.gridMap?.origin) {
|
|
const dx = Math.abs(this.gridMap.origin.x - this.lockedMapOrigin.x);
|
|
const dy = Math.abs(this.gridMap.origin.y - this.lockedMapOrigin.y);
|
|
const dz = Math.abs(this.gridMap.origin.z - this.lockedMapOrigin.z);
|
|
|
|
if (dx > 0.001 || dy > 0.001 || dz > 0.001) {
|
|
console.error('⚠️ CRITICAL: Map origin changed during render! Restoring locked origin.');
|
|
console.error(' Locked:', this.lockedMapOrigin);
|
|
console.error(' Current:', this.gridMap.origin);
|
|
this.gridMap.origin = {
|
|
x: this.lockedMapOrigin.x,
|
|
y: this.lockedMapOrigin.y,
|
|
z: this.lockedMapOrigin.z
|
|
};
|
|
}
|
|
}
|
|
|
|
// Clear canvas - NO white background, only grid will show
|
|
this.ctx.clearRect(0, 0, this.canvas.width, this.canvas.height);
|
|
|
|
// Draw in order: grid -> grid map -> map frame axes -> cost map -> global path -> laser scans -> initial pose -> robot pose
|
|
// Cost map and path should be drawn BEFORE laser scans so they're visible underneath
|
|
this.drawGrid();
|
|
this.drawGridMap();
|
|
this.drawCoordinateAxes(); // Draw map frame axes (X=red, Y=green) at origin
|
|
this.drawWorldCoordinateAxes(); // Draw world coordinate axes (X=red, Y=green) at origin
|
|
this.drawCostMap(); // Draw cost map overlay
|
|
this.drawGlobalPath(); // Draw path BEFORE laser scans
|
|
this.drawLocalPath(); // Draw local path BEFORE laser scans
|
|
this.drawLaserScans();
|
|
this.drawInitialPose();
|
|
if (this.showRobotPose) {
|
|
this.drawRobotPose();
|
|
}
|
|
this.drawRobotAxes();
|
|
this.drawRobotFootprint();
|
|
|
|
// CRITICAL: Use locked view offsets if available (prevents map frame from moving)
|
|
if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
|
|
// Force use locked offsets - map frame MUST stay fixed
|
|
if (Math.abs(this.offsetX - this.lockedOffsetX) > 0.1 ||
|
|
Math.abs(this.offsetY - this.lockedOffsetY) > 0.1) {
|
|
console.error('⚠️ CRITICAL ERROR: View offsets changed during render!');
|
|
console.error(' Locked:', { offsetX: this.lockedOffsetX, offsetY: this.lockedOffsetY });
|
|
console.error(' Current:', { offsetX: this.offsetX, offsetY: this.offsetY });
|
|
console.error(' Robot pose:', this.robotPose);
|
|
console.error(' → Restoring locked offsets!');
|
|
// Restore locked offsets
|
|
this.offsetX = this.lockedOffsetX;
|
|
this.offsetY = this.lockedOffsetY;
|
|
}
|
|
}
|
|
},
|
|
|
|
/**
|
|
* Draw background grid (1 meter squares). Grid origin = Map frame = canvas center.
|
|
*/
|
|
drawGrid: function() {
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
|
|
const gridSpacing = this.scale;
|
|
const snapToPixel = (value) => Math.round(value) + 0.5;
|
|
|
|
// Calculate visible range in map frame coordinates
|
|
// For X: canvas X = mapFrameX + (mapX * scale), so mapX = (canvasX - mapFrameX) / scale
|
|
const minX = (-mapFrameX) / this.scale;
|
|
const maxX = (this.canvas.width - mapFrameX) / this.scale;
|
|
|
|
// For Y: canvas Y = mapFrameY - (mapY * scale), so mapY = (mapFrameY - canvasY) / scale
|
|
// Top of canvas (y=0) corresponds to: mapY = (mapFrameY - 0) / scale = mapFrameY / scale
|
|
// Bottom of canvas (y=height) corresponds to: mapY = (mapFrameY - height) / scale
|
|
const minY = (mapFrameY - this.canvas.height) / this.scale;
|
|
const maxY = mapFrameY / this.scale;
|
|
|
|
// Add margin to ensure all visible grid lines are drawn
|
|
const margin = 3; // meters - increased to ensure coverage
|
|
const startGridX = Math.floor(minX - margin);
|
|
const endGridX = Math.ceil(maxX + margin);
|
|
const startGridY = Math.floor(minY - margin);
|
|
const endGridY = Math.ceil(maxY + margin);
|
|
|
|
// Adjust line width based on zoom level for better visibility
|
|
const baseLineWidth = 1;
|
|
const adjustedLineWidth = Math.max(0.5, Math.min(2, baseLineWidth / (this.scale / 20)));
|
|
|
|
// Draw vertical lines (1 meter spacing) - relative to World frame
|
|
this.ctx.strokeStyle = '#d0d0d0'; // Darker gray
|
|
this.ctx.lineWidth = adjustedLineWidth;
|
|
this.ctx.globalAlpha = 0.6; // More opaque
|
|
|
|
for (let x = startGridX; x <= endGridX; x += 1) {
|
|
const canvasX = snapToPixel(mapFrameX + (x * gridSpacing));
|
|
// Use larger margin to ensure lines are drawn even when slightly outside viewport
|
|
if (canvasX >= -100 && canvasX <= this.canvas.width + 100) {
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(canvasX, 0);
|
|
this.ctx.lineTo(canvasX, this.canvas.height);
|
|
this.ctx.stroke();
|
|
}
|
|
}
|
|
|
|
// Draw horizontal lines (1 meter spacing) - relative to World frame
|
|
// Important: y increases upward in map frame, but canvas Y increases downward
|
|
// So for map frame y, canvas Y = mapFrameY - (y * gridSpacing)
|
|
for (let y = startGridY; y <= endGridY; y += 1) {
|
|
const canvasY = snapToPixel(mapFrameY - (y * gridSpacing));
|
|
// Use much larger margin and remove strict bounds checking to ensure all lines are drawn
|
|
// The key is to draw all lines in the calculated range, regardless of exact canvas position
|
|
if (!isNaN(canvasY) && isFinite(canvasY)) {
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(0, canvasY);
|
|
this.ctx.lineTo(this.canvas.width, canvasY);
|
|
this.ctx.stroke();
|
|
}
|
|
}
|
|
|
|
// Draw 5-meter grid lines (darker and more visible)
|
|
this.ctx.strokeStyle = '#b0b0b0'; // Even darker
|
|
this.ctx.lineWidth = Math.max(1, adjustedLineWidth * 2);
|
|
this.ctx.globalAlpha = 0.7;
|
|
|
|
for (let x = startGridX; x <= endGridX; x += 1) {
|
|
if (x % 5 === 0) {
|
|
const canvasX = snapToPixel(mapFrameX + (x * gridSpacing));
|
|
if (canvasX >= -100 && canvasX <= this.canvas.width + 100) {
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(canvasX, 0);
|
|
this.ctx.lineTo(canvasX, this.canvas.height);
|
|
this.ctx.stroke();
|
|
}
|
|
}
|
|
}
|
|
|
|
for (let y = startGridY; y <= endGridY; y += 1) {
|
|
if (y % 5 === 0) {
|
|
const canvasY = snapToPixel(mapFrameY - (y * gridSpacing));
|
|
// Remove bounds checking for 5-meter lines too - just check for valid number
|
|
if (!isNaN(canvasY) && isFinite(canvasY)) {
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(0, canvasY);
|
|
this.ctx.lineTo(this.canvas.width, canvasY);
|
|
this.ctx.stroke();
|
|
}
|
|
}
|
|
}
|
|
|
|
// Reset alpha for other drawing
|
|
this.ctx.globalAlpha = 1.0;
|
|
},
|
|
|
|
/**
|
|
* Draw the occupancy grid map - only renders occupied cells (100) as black boundaries
|
|
*/
|
|
drawGridMap: function() {
|
|
if (!this.gridMap) {
|
|
console.debug('[drawGridMap] No gridMap data');
|
|
return;
|
|
}
|
|
if (!this.mapCacheReady) {
|
|
console.debug('[drawGridMap] Map cache not ready');
|
|
return;
|
|
}
|
|
if (!this.mapCacheCanvas) {
|
|
console.debug('[drawGridMap] Map cache canvas not available');
|
|
return;
|
|
}
|
|
|
|
try {
|
|
const width = this.gridMap.width;
|
|
const height = this.gridMap.height;
|
|
const resolution = this.gridMap.resolution;
|
|
|
|
// Map frame (0,0) = canvas center
|
|
// Origin is the position of cell [0,0] (bottom-left corner) in map frame coordinates
|
|
// In ROS occupancy grid: origin is the world position of the bottom-left corner of cell [0,0]
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
const originXY = this.getMapOriginXY();
|
|
|
|
// Calculate where cell [0,0] should be drawn in canvas coordinates
|
|
// Map frame origin (0,0) is at (mapFrameX, mapFrameY) in canvas
|
|
// Cell [0,0] is at (origin.x, origin.y) in map frame
|
|
// Canvas coordinates: mapFrameX + origin.x * scale, mapFrameY - origin.y * scale (flip Y)
|
|
const cell00CanvasX = mapFrameX + (originXY.x * this.scale);
|
|
const cell00CanvasY = mapFrameY - (originXY.y * this.scale);
|
|
|
|
const mapWidthPixels = width * resolution * this.scale;
|
|
const mapHeightPixels = height * resolution * this.scale;
|
|
|
|
this.ctx.save();
|
|
// Translate to where cell [0,0] should be drawn
|
|
this.ctx.translate(cell00CanvasX, cell00CanvasY);
|
|
// Flip Y axis (map Y increases upward, canvas Y increases downward)
|
|
// After flip, cell [0,0] is at bottom-left, map extends right and up
|
|
this.ctx.scale(1, -1);
|
|
// Draw map: cell (0,0) will be at the translate point after flip
|
|
// Map extends rightward (positive X) and upward (positive Y in map frame, downward in canvas after flip)
|
|
this.ctx.drawImage(this.mapCacheCanvas, 0, 0, mapWidthPixels, mapHeightPixels);
|
|
this.ctx.restore();
|
|
|
|
} catch (error) {
|
|
console.error('[drawGridMap] Error drawing grid map:', error);
|
|
}
|
|
},
|
|
|
|
/**
|
|
* Draw robot pose icon (position and orientation) as a translucent purple arrowhead.
|
|
*/
|
|
drawRobotPose: function() {
|
|
if (!this.robotPose) return;
|
|
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
// Robot pose in MAP coordinates -> canvas = mapFrame + (robot.x*scale, -robot.y*scale)
|
|
const robotX = mapFrameX + (this.robotPose.x * this.scale);
|
|
const robotY = mapFrameY - (this.robotPose.y * this.scale);
|
|
|
|
const arrowLength = 35;
|
|
const headAngle = Math.PI / 6;
|
|
const angle = -this.robotPose.yaw;
|
|
const arrowTipX = robotX + arrowLength * Math.cos(angle);
|
|
const arrowTipY = robotY + arrowLength * Math.sin(angle);
|
|
|
|
this.ctx.fillStyle = 'rgba(156, 39, 176, 0.45)';
|
|
this.ctx.strokeStyle = 'rgba(106, 27, 154, 0.85)';
|
|
this.ctx.lineWidth = 3;
|
|
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(arrowTipX, arrowTipY);
|
|
this.ctx.lineTo(
|
|
robotX - arrowLength * 0.5 * Math.cos(angle - headAngle),
|
|
robotY - arrowLength * 0.5 * Math.sin(angle - headAngle)
|
|
);
|
|
this.ctx.lineTo(
|
|
robotX - arrowLength * 0.5 * Math.cos(angle + headAngle),
|
|
robotY - arrowLength * 0.5 * Math.sin(angle + headAngle)
|
|
);
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
this.ctx.stroke();
|
|
},
|
|
|
|
/**
|
|
* Draw robot-local coordinate axes independently from the robot icon toggle.
|
|
*/
|
|
drawRobotAxes: function() {
|
|
if (!this.robotPose) return;
|
|
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
const robotX = mapFrameX + (this.robotPose.x * this.scale);
|
|
const robotY = mapFrameY - (this.robotPose.y * this.scale);
|
|
this.drawRobotCoordinateAxes(robotX, robotY, this.robotPose.yaw || 0);
|
|
},
|
|
|
|
/**
|
|
* Draw robot-local coordinate axes (x: red, y: green) at robot center.
|
|
*/
|
|
drawRobotCoordinateAxes: function(robotX, robotY, yaw) {
|
|
const axisLength = 42;
|
|
const arrowHeadSize = 8;
|
|
|
|
// Convert robot yaw (map convention) to canvas angle convention.
|
|
const xAxisAngle = -yaw;
|
|
const yAxisAngle = xAxisAngle - Math.PI / 2;
|
|
|
|
const xEndX = robotX + axisLength * Math.cos(xAxisAngle);
|
|
const xEndY = robotY + axisLength * Math.sin(xAxisAngle);
|
|
const yEndX = robotX + axisLength * Math.cos(yAxisAngle);
|
|
const yEndY = robotY + axisLength * Math.sin(yAxisAngle);
|
|
|
|
// X axis
|
|
this.ctx.strokeStyle = '#F44336';
|
|
this.ctx.lineWidth = 2.5;
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(robotX, robotY);
|
|
this.ctx.lineTo(xEndX, xEndY);
|
|
this.ctx.stroke();
|
|
|
|
this.ctx.fillStyle = '#F44336';
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(xEndX, xEndY);
|
|
this.ctx.lineTo(
|
|
xEndX - arrowHeadSize * Math.cos(xAxisAngle - Math.PI / 6),
|
|
xEndY - arrowHeadSize * Math.sin(xAxisAngle - Math.PI / 6)
|
|
);
|
|
this.ctx.lineTo(
|
|
xEndX - arrowHeadSize * Math.cos(xAxisAngle + Math.PI / 6),
|
|
xEndY - arrowHeadSize * Math.sin(xAxisAngle + Math.PI / 6)
|
|
);
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
|
|
// Y axis
|
|
this.ctx.strokeStyle = '#4CAF50';
|
|
this.ctx.lineWidth = 2.5;
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(robotX, robotY);
|
|
this.ctx.lineTo(yEndX, yEndY);
|
|
this.ctx.stroke();
|
|
|
|
this.ctx.fillStyle = '#4CAF50';
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(yEndX, yEndY);
|
|
this.ctx.lineTo(
|
|
yEndX - arrowHeadSize * Math.cos(yAxisAngle - Math.PI / 6),
|
|
yEndY - arrowHeadSize * Math.sin(yAxisAngle - Math.PI / 6)
|
|
);
|
|
this.ctx.lineTo(
|
|
yEndX - arrowHeadSize * Math.cos(yAxisAngle + Math.PI / 6),
|
|
yEndY - arrowHeadSize * Math.sin(yAxisAngle + Math.PI / 6)
|
|
);
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
|
|
// Axis labels
|
|
this.ctx.font = 'bold 12px Arial';
|
|
this.ctx.fillStyle = '#F44336';
|
|
this.ctx.fillText('x', xEndX + 6, xEndY + 4);
|
|
this.ctx.fillStyle = '#4CAF50';
|
|
this.ctx.fillText('y', yEndX + 6, yEndY + 4);
|
|
},
|
|
|
|
/**
|
|
* Draw robot footprint (base outline) at current robot pose.
|
|
* Transforms footprint points from robot frame to map frame.
|
|
*/
|
|
drawRobotFootprint: function() {
|
|
if (!this.showRobotFootprint || !this.robotPose || !this.robotFootprint || this.robotFootprint.length === 0) {
|
|
return;
|
|
}
|
|
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
const robotX = this.robotPose.x;
|
|
const robotY = this.robotPose.y;
|
|
const yaw = this.robotPose.yaw || 0;
|
|
|
|
const cos = Math.cos(yaw);
|
|
const sin = Math.sin(yaw);
|
|
|
|
// Transform footprint points from robot frame to map frame
|
|
const transformedPoints = this.robotFootprint.map(point => {
|
|
// Rotate and translate point
|
|
const rotatedX = point.x * cos - point.y * sin;
|
|
const rotatedY = point.x * sin + point.y * cos;
|
|
const mapX = robotX + rotatedX;
|
|
const mapY = robotY + rotatedY;
|
|
|
|
// Convert to canvas coordinates
|
|
const canvasX = mapFrameX + (mapX * this.scale);
|
|
const canvasY = mapFrameY - (mapY * this.scale);
|
|
return { x: canvasX, y: canvasY, origX: point.x, origY: point.y };
|
|
});
|
|
|
|
// Sort points by angle from center to ensure correct square drawing order
|
|
if (transformedPoints.length >= 4) {
|
|
// Calculate center of footprint in robot frame
|
|
const centerX = this.robotFootprint.reduce((sum, p) => sum + p.x, 0) / this.robotFootprint.length;
|
|
const centerY = this.robotFootprint.reduce((sum, p) => sum + p.y, 0) / this.robotFootprint.length;
|
|
|
|
// Sort by angle from center (counterclockwise)
|
|
transformedPoints.sort((a, b) => {
|
|
const angleA = Math.atan2(a.origY - centerY, a.origX - centerX);
|
|
const angleB = Math.atan2(b.origY - centerY, b.origX - centerX);
|
|
return angleA - angleB;
|
|
});
|
|
}
|
|
|
|
// Draw footprint as a square
|
|
this.ctx.fillStyle = 'rgba(0, 188, 212, 0.2)'; // Cyan with transparency
|
|
this.ctx.strokeStyle = '#00bcd4'; // Cyan color
|
|
this.ctx.lineWidth = 2;
|
|
this.ctx.beginPath();
|
|
|
|
// Draw square by connecting all points in sorted order
|
|
if (transformedPoints.length >= 4) {
|
|
this.ctx.moveTo(transformedPoints[0].x, transformedPoints[0].y);
|
|
for (let i = 1; i < transformedPoints.length; i++) {
|
|
this.ctx.lineTo(transformedPoints[i].x, transformedPoints[i].y);
|
|
}
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
this.ctx.stroke();
|
|
}
|
|
},
|
|
|
|
/**
|
|
* Draw initial pose (position + heading) as a bright blue arrow
|
|
*/
|
|
drawInitialPose: function() {
|
|
// Only draw if mode is enabled
|
|
if (!this.initialPoseMode) return;
|
|
|
|
// If no initial pose exists, don't draw (should not happen if mode is enabled)
|
|
if (!this.initialPose) {
|
|
console.warn("Initial pose mode is ON but no initial pose exists");
|
|
return;
|
|
}
|
|
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
const baseX = mapFrameX + (this.initialPose.x * this.scale);
|
|
const baseY = mapFrameY - (this.initialPose.y * this.scale);
|
|
|
|
const arrowLength = 50; // Larger arrow
|
|
const headAngle = Math.PI / 6;
|
|
const angle = -this.initialPose.yaw;
|
|
|
|
const tipX = baseX + arrowLength * Math.cos(angle);
|
|
const tipY = baseY + arrowLength * Math.sin(angle);
|
|
|
|
// Draw a glowing effect (outer glow) - more visible when dragging
|
|
if (this.initialPoseDragging) {
|
|
this.ctx.shadowColor = 'rgba(33, 150, 243, 1.0)';
|
|
this.ctx.shadowBlur = 20;
|
|
} else {
|
|
this.ctx.shadowColor = 'rgba(33, 150, 243, 0.8)';
|
|
this.ctx.shadowBlur = 15;
|
|
}
|
|
|
|
// Base marker (larger and bright blue)
|
|
this.ctx.fillStyle = '#2196F3'; // Material Blue
|
|
this.ctx.strokeStyle = '#1976D2';
|
|
this.ctx.lineWidth = this.initialPoseDragging ? 4 : 3;
|
|
this.ctx.beginPath();
|
|
this.ctx.arc(baseX, baseY, this.initialPoseDragging ? 10 : 8, 0, 2 * Math.PI);
|
|
this.ctx.fill();
|
|
this.ctx.stroke();
|
|
|
|
// Arrow shaft (thicker)
|
|
this.ctx.strokeStyle = '#2196F3';
|
|
this.ctx.lineWidth = this.initialPoseDragging ? 5 : 4;
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(baseX, baseY);
|
|
this.ctx.lineTo(tipX, tipY);
|
|
this.ctx.stroke();
|
|
|
|
// Arrow head (larger)
|
|
this.ctx.fillStyle = '#2196F3';
|
|
this.ctx.strokeStyle = '#1976D2';
|
|
this.ctx.lineWidth = this.initialPoseDragging ? 3 : 2;
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(tipX, tipY);
|
|
this.ctx.lineTo(
|
|
tipX - arrowLength * 0.5 * Math.cos(angle - headAngle),
|
|
tipY - arrowLength * 0.5 * Math.sin(angle - headAngle)
|
|
);
|
|
this.ctx.lineTo(
|
|
tipX - arrowLength * 0.5 * Math.cos(angle + headAngle),
|
|
tipY - arrowLength * 0.5 * Math.sin(angle + headAngle)
|
|
);
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
this.ctx.stroke();
|
|
|
|
// Reset shadow
|
|
this.ctx.shadowBlur = 0;
|
|
},
|
|
|
|
/**
|
|
* Draw laser scan points from all LIDARs
|
|
*/
|
|
drawLaserScans: function() {
|
|
if (!this.laserScanData) return;
|
|
|
|
const lidar1Options = this.lidarDisplayOptions?.lidar1 || { visible: true, mode: 'minimal' };
|
|
const lidar2Options = this.lidarDisplayOptions?.lidar2 || { visible: true, mode: 'minimal' };
|
|
const lidar3Options = this.lidarDisplayOptions?.lidar3 || { visible: true, mode: 'minimal' };
|
|
|
|
// Draw each LIDAR's scan with different colors (darker for white background)
|
|
if (lidar1Options.visible && this.laserScanData.lidar1) {
|
|
this.drawSingleLidarScan(this.laserScanData.lidar1, '#cc0000', this.getLidarTransform('scan_1'), lidar1Options.mode); // Dark Red
|
|
}
|
|
if (lidar2Options.visible && this.laserScanData.lidar2) {
|
|
this.drawSingleLidarScan(this.laserScanData.lidar2, '#00aa00', this.getLidarTransform('scan_2'), lidar2Options.mode); // Dark Green
|
|
}
|
|
if (lidar3Options.visible && this.laserScanData.lidar3) {
|
|
this.drawSingleLidarScan(this.laserScanData.lidar3, '#0000cc', this.getLidarTransform('scan_3'), lidar3Options.mode); // Dark Blue
|
|
}
|
|
},
|
|
|
|
drawGlobalPath: function() {
|
|
if (!this.showGlobalPath) return;
|
|
if (!this.globalPathData || this.globalPathData.length === 0) return;
|
|
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
|
|
const pathPoints = this.globalPathData.map(point => ({
|
|
x: mapFrameX + (point.x * this.scale),
|
|
y: mapFrameY - (point.y * this.scale)
|
|
}));
|
|
|
|
// Check if any points are visible
|
|
const visiblePoints = pathPoints.filter(p =>
|
|
p.x >= 0 && p.x <= this.canvas.width &&
|
|
p.y >= 0 && p.y <= this.canvas.height
|
|
);
|
|
|
|
if (visiblePoints.length === 0) {
|
|
console.warn('Global path: no points visible in current view (pan/zoom or path off-screen).');
|
|
}
|
|
|
|
this.ctx.save();
|
|
|
|
// Draw path line - RViz style with green color
|
|
if (pathPoints.length > 1) {
|
|
// Background white outline for visibility
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(pathPoints[0].x, pathPoints[0].y);
|
|
for (let i = 1; i < pathPoints.length; i++) {
|
|
this.ctx.lineTo(pathPoints[i].x, pathPoints[i].y);
|
|
}
|
|
this.ctx.strokeStyle = '#FFFFFF';
|
|
this.ctx.lineWidth = 8;
|
|
this.ctx.lineJoin = 'round';
|
|
this.ctx.lineCap = 'round';
|
|
this.ctx.globalAlpha = 0.9;
|
|
this.ctx.stroke();
|
|
|
|
// Main path line - bright green like RViz
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(pathPoints[0].x, pathPoints[0].y);
|
|
for (let i = 1; i < pathPoints.length; i++) {
|
|
this.ctx.lineTo(pathPoints[i].x, pathPoints[i].y);
|
|
}
|
|
this.ctx.strokeStyle = '#00FF00'; // Bright green like RViz
|
|
this.ctx.lineWidth = 4;
|
|
this.ctx.globalAlpha = 0.85;
|
|
this.ctx.stroke();
|
|
}
|
|
|
|
// Draw path points for short paths or better visibility
|
|
this.ctx.globalAlpha = 0.8;
|
|
const pointStep = Math.max(1, Math.floor(pathPoints.length / 50));
|
|
this.ctx.fillStyle = '#00FF00';
|
|
for (let i = 0; i < pathPoints.length; i += pointStep) {
|
|
const point = pathPoints[i];
|
|
this.ctx.beginPath();
|
|
this.ctx.arc(point.x, point.y, 3, 0, 2 * Math.PI);
|
|
this.ctx.fill();
|
|
}
|
|
|
|
// Directional arrows along the path (heading indicators)
|
|
if (pathPoints.length > 1) {
|
|
const arrowSpacingPx = Math.max(30, this.scale * 0.8); // ~1 arrow per 0.8 m (min 30px)
|
|
const arrowSize = Math.max(5, Math.min(12, this.scale * 0.18));
|
|
|
|
this.ctx.globalAlpha = 0.95;
|
|
this.ctx.fillStyle = '#FF0000';
|
|
this.ctx.strokeStyle = '#FF0000';
|
|
this.ctx.lineWidth = 1;
|
|
|
|
let distAccum = arrowSpacingPx * 0.5; // first arrow offset
|
|
|
|
for (let i = 0; i < pathPoints.length - 1; i++) {
|
|
const p0 = pathPoints[i];
|
|
const p1 = pathPoints[i + 1];
|
|
const dx = p1.x - p0.x;
|
|
const dy = p1.y - p0.y;
|
|
const segLen = Math.sqrt(dx * dx + dy * dy);
|
|
if (segLen < 1e-6) continue;
|
|
|
|
let remaining = segLen;
|
|
let traveled = 0;
|
|
|
|
while (distAccum <= remaining) {
|
|
const t = (traveled + distAccum) / segLen;
|
|
const px = p0.x + dx * t;
|
|
const py = p0.y + dy * t;
|
|
|
|
// Prefer theta from path data (world frame -> flip Y for screen)
|
|
const worldTheta = this.globalPathData[i]?.theta;
|
|
const angle = (worldTheta !== undefined && isFinite(worldTheta))
|
|
? -worldTheta
|
|
: Math.atan2(dy, dx);
|
|
|
|
const L = arrowSize;
|
|
const W = arrowSize * 0.55;
|
|
|
|
this.ctx.save();
|
|
this.ctx.translate(px, py);
|
|
this.ctx.rotate(angle);
|
|
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo( L, 0); // tip
|
|
this.ctx.lineTo(-L * 0.6, -W); // left wing
|
|
this.ctx.lineTo(-L * 0.25, 0); // tail notch
|
|
this.ctx.lineTo(-L * 0.6, W); // right wing
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
this.ctx.stroke();
|
|
|
|
this.ctx.restore();
|
|
|
|
traveled += distAccum;
|
|
remaining -= distAccum;
|
|
distAccum = arrowSpacingPx;
|
|
}
|
|
|
|
distAccum -= remaining;
|
|
}
|
|
}
|
|
|
|
// Start marker - bright green circle
|
|
const firstPoint = pathPoints[0];
|
|
this.ctx.globalAlpha = 1.0;
|
|
this.ctx.fillStyle = '#FFFFFF';
|
|
this.ctx.beginPath();
|
|
this.ctx.arc(firstPoint.x, firstPoint.y, 8, 0, 2 * Math.PI);
|
|
this.ctx.fill();
|
|
this.ctx.fillStyle = '#00FF00';
|
|
this.ctx.beginPath();
|
|
this.ctx.arc(firstPoint.x, firstPoint.y, 6, 0, 2 * Math.PI);
|
|
this.ctx.fill();
|
|
|
|
// End marker - bright red circle
|
|
const lastPoint = pathPoints[pathPoints.length - 1];
|
|
this.ctx.fillStyle = '#FFFFFF';
|
|
this.ctx.beginPath();
|
|
this.ctx.arc(lastPoint.x, lastPoint.y, 8, 0, 2 * Math.PI);
|
|
this.ctx.fill();
|
|
this.ctx.fillStyle = '#FF0000';
|
|
this.ctx.beginPath();
|
|
this.ctx.arc(lastPoint.x, lastPoint.y, 6, 0, 2 * Math.PI);
|
|
this.ctx.fill();
|
|
|
|
this.ctx.restore();
|
|
},
|
|
|
|
drawLocalPath: function() {
|
|
if (!this.showLocalPath) return;
|
|
if (!this.localPathData || this.localPathData.length < 2) return;
|
|
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
const pathPoints = this.localPathData.map(point => ({
|
|
x: mapFrameX + (point.x * this.scale),
|
|
y: mapFrameY - (point.y * this.scale)
|
|
}));
|
|
|
|
this.ctx.save();
|
|
// White outline for contrast
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(pathPoints[0].x, pathPoints[0].y);
|
|
for (let i = 1; i < pathPoints.length; i++) {
|
|
this.ctx.lineTo(pathPoints[i].x, pathPoints[i].y);
|
|
}
|
|
this.ctx.strokeStyle = '#FFFFFF';
|
|
this.ctx.lineWidth = 6;
|
|
this.ctx.lineJoin = 'round';
|
|
this.ctx.lineCap = 'round';
|
|
this.ctx.globalAlpha = 0.85;
|
|
this.ctx.stroke();
|
|
|
|
// Main local path line (orange)
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(pathPoints[0].x, pathPoints[0].y);
|
|
for (let i = 1; i < pathPoints.length; i++) {
|
|
this.ctx.lineTo(pathPoints[i].x, pathPoints[i].y);
|
|
}
|
|
this.ctx.strokeStyle = '#ff9800';
|
|
this.ctx.lineWidth = 3;
|
|
this.ctx.globalAlpha = 0.9;
|
|
this.ctx.stroke();
|
|
|
|
// Directional arrows along the local path
|
|
if (pathPoints.length > 1) {
|
|
const arrowSpacingPx = Math.max(25, this.scale * 0.5);
|
|
const arrowSize = Math.max(4, Math.min(10, this.scale * 0.15));
|
|
|
|
this.ctx.globalAlpha = 0.95;
|
|
this.ctx.fillStyle = '#ff9800';
|
|
this.ctx.strokeStyle = '#b26a00';
|
|
this.ctx.lineWidth = 1;
|
|
|
|
let distAccum = arrowSpacingPx * 0.5;
|
|
|
|
for (let i = 0; i < pathPoints.length - 1; i++) {
|
|
const p0 = pathPoints[i];
|
|
const p1 = pathPoints[i + 1];
|
|
const dx = p1.x - p0.x;
|
|
const dy = p1.y - p0.y;
|
|
const segLen = Math.sqrt(dx * dx + dy * dy);
|
|
if (segLen < 1e-6) continue;
|
|
|
|
let remaining = segLen;
|
|
let traveled = 0;
|
|
|
|
while (distAccum <= remaining) {
|
|
const t = (traveled + distAccum) / segLen;
|
|
const px = p0.x + dx * t;
|
|
const py = p0.y + dy * t;
|
|
|
|
const worldTheta = this.localPathData[i]?.theta;
|
|
const angle = (worldTheta !== undefined && isFinite(worldTheta))
|
|
? -worldTheta
|
|
: Math.atan2(dy, dx);
|
|
|
|
const L = arrowSize;
|
|
const W = arrowSize * 0.55;
|
|
|
|
this.ctx.save();
|
|
this.ctx.translate(px, py);
|
|
this.ctx.rotate(angle);
|
|
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo( L, 0);
|
|
this.ctx.lineTo(-L * 0.6, -W);
|
|
this.ctx.lineTo(-L * 0.25, 0);
|
|
this.ctx.lineTo(-L * 0.6, W);
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
this.ctx.stroke();
|
|
|
|
this.ctx.restore();
|
|
|
|
traveled += distAccum;
|
|
remaining -= distAccum;
|
|
distAccum = arrowSpacingPx;
|
|
}
|
|
|
|
distAccum -= remaining;
|
|
}
|
|
}
|
|
|
|
this.ctx.restore();
|
|
},
|
|
|
|
// ==================== COST MAP FUNCTIONS ====================
|
|
|
|
setCostMapVisible: function(visible) {
|
|
this.showCostMap = !!visible;
|
|
console.log(`🔄 Cost Map visibility: ${this.showCostMap ? 'ENABLED' : 'DISABLED'}`);
|
|
this.requestRender();
|
|
},
|
|
|
|
setRobotPoseVisible: function(visible) {
|
|
this.showRobotPose = !!visible;
|
|
console.log(`🔄 Robot icon visibility: ${this.showRobotPose ? 'ENABLED' : 'DISABLED'}`);
|
|
this.requestRender();
|
|
},
|
|
|
|
setRobotFootprint: function(footprintJson) {
|
|
try {
|
|
const footprintData = typeof footprintJson === 'string' ? JSON.parse(footprintJson) : footprintJson;
|
|
this.robotFootprint = footprintData;
|
|
console.log('📐 Robot footprint set:', this.robotFootprint);
|
|
this.requestRender();
|
|
} catch (error) {
|
|
console.error('❌ Failed to parse robot footprint:', error);
|
|
this.robotFootprint = null;
|
|
}
|
|
},
|
|
|
|
setRobotFootprintVisible: function(visible) {
|
|
this.showRobotFootprint = !!visible;
|
|
console.log(`🔄 Robot footprint visibility: ${this.showRobotFootprint ? 'ENABLED' : 'DISABLED'}`);
|
|
this.requestRender();
|
|
},
|
|
|
|
setCostMapData: function(data) {
|
|
if (!data) {
|
|
return;
|
|
}
|
|
|
|
try {
|
|
const hasFullMap = data.hasFullMap === true || (!!data.dataBase64 && data.width > 0 && data.height > 0);
|
|
const isCostmapUpdated = data.isCostmapUpdated === true || !!data.costmapUpdate;
|
|
|
|
if (!hasFullMap && !isCostmapUpdated) {
|
|
return;
|
|
}
|
|
|
|
if (hasFullMap) {
|
|
const width = Number(data.width) || 0;
|
|
const height = Number(data.height) || 0;
|
|
const resolution = Number(data.resolution) || 0;
|
|
const originX = Number(data.originX) || 0;
|
|
const originY = Number(data.originY) || 0;
|
|
const originTheta = Number(data.originTheta) || 0;
|
|
if (width <= 0 || height <= 0 || resolution <= 0) {
|
|
console.warn('[setCostMapData] Invalid full costmap payload');
|
|
return;
|
|
}
|
|
|
|
const fullData = this.decodeCostMapBase64(data.dataBase64);
|
|
if (!fullData || fullData.length === 0) {
|
|
console.warn('[setCostMapData] Missing full costmap bytes');
|
|
return;
|
|
}
|
|
const hasExistingSnapshot = !!(this.costMapData && this.costMapData.data);
|
|
const dimensionsChanged = !hasExistingSnapshot ||
|
|
this.costMapData.width !== width ||
|
|
this.costMapData.height !== height ||
|
|
Math.abs(this.costMapData.resolution - resolution) > 1e-9;
|
|
|
|
// Always update costmap data for continuous updates (lidar frequency)
|
|
this.costMapData = {
|
|
frameId: data.frameId || 'map',
|
|
width: width,
|
|
height: height,
|
|
resolution: resolution,
|
|
originX: originX,
|
|
originY: originY,
|
|
originTheta: originTheta,
|
|
poseX: dimensionsChanged ? originX : this.costMapData.poseX,
|
|
poseY: dimensionsChanged ? originY : this.costMapData.poseY,
|
|
fixedYaw: dimensionsChanged
|
|
? (Number.isFinite(this.costMapFixedYaw) ? this.costMapFixedYaw : originTheta)
|
|
: this.costMapData.fixedYaw,
|
|
data: fullData
|
|
};
|
|
|
|
if (dimensionsChanged) {
|
|
this.costMapPoseAnchor = { x: originX, y: originY };
|
|
this.costMapOdomAnchor = null;
|
|
}
|
|
|
|
this.rebuildCostMapCache();
|
|
}
|
|
|
|
if (this.costMapData && isCostmapUpdated) {
|
|
this.applyCostMapUpdate(this.costMapData, data.costmapUpdate);
|
|
}
|
|
|
|
this.updateCostMapPoseFromOdometry(
|
|
data.odometry,
|
|
data.localizationActive === true,
|
|
data.xlocPose,
|
|
Number(data.matchingScore)
|
|
);
|
|
|
|
if (this.costMapData && Number.isFinite(this.costMapFixedYaw)) {
|
|
this.costMapData.fixedYaw = this.costMapFixedYaw;
|
|
}
|
|
|
|
if (data.xlocPose && Number(data.matchingScore) !== -1) {
|
|
this.setRobotPose(data.xlocPose);
|
|
}
|
|
|
|
this.requestRender();
|
|
} catch (error) {
|
|
console.error('❌ Error processing cost map data:', error);
|
|
}
|
|
},
|
|
|
|
decodeCostMapBase64: function(dataBase64) {
|
|
if (!dataBase64) {
|
|
return null;
|
|
}
|
|
const binaryString = atob(dataBase64);
|
|
const bytes = new Uint8Array(binaryString.length);
|
|
for (let i = 0; i < binaryString.length; i++) {
|
|
bytes[i] = binaryString.charCodeAt(i);
|
|
}
|
|
return bytes;
|
|
},
|
|
|
|
rebuildCostMapCache: function() {
|
|
if (!this.costMapData || !this.costMapData.data) {
|
|
this.costMapCacheReady = false;
|
|
return;
|
|
}
|
|
|
|
const { width, height, data } = this.costMapData;
|
|
|
|
if (!this.costMapCacheCanvas) {
|
|
this.costMapCacheCanvas = document.createElement('canvas');
|
|
this.costMapCacheCtx = this.costMapCacheCanvas.getContext('2d');
|
|
}
|
|
|
|
// Rotate 90° clockwise: swap width and height
|
|
this.costMapCacheCanvas.width = height;
|
|
this.costMapCacheCanvas.height = width;
|
|
|
|
const imageData = this.costMapCacheCtx.createImageData(height, width);
|
|
const pixels = imageData.data;
|
|
|
|
// Rotate 90° clockwise: original (x, y) -> rotated (height - 1 - y, x)
|
|
for (let y = 0; y < height; y++) {
|
|
for (let x = 0; x < width; x++) {
|
|
const srcIdx = y * width + x;
|
|
const costValue = data[srcIdx];
|
|
|
|
// Rotated coordinates
|
|
const rotX = height - 1 - y;
|
|
const rotY = x;
|
|
const dstIdx = rotY * height + rotX;
|
|
const pxIdx = dstIdx * 4;
|
|
|
|
// Unknown and free space should stay transparent so inflation gradient is visible.
|
|
if (costValue === 255 || costValue === 0) {
|
|
pixels[pxIdx] = 0;
|
|
pixels[pxIdx + 1] = 0;
|
|
pixels[pxIdx + 2] = 0;
|
|
pixels[pxIdx + 3] = 0;
|
|
continue;
|
|
}
|
|
|
|
let r = 0;
|
|
let g = 0;
|
|
let b = 0;
|
|
let a = 0;
|
|
|
|
if (costValue >= 253) {
|
|
// Inscribed/lethal obstacles.
|
|
r = 255;
|
|
g = 30;
|
|
b = 30;
|
|
a = 235;
|
|
} else {
|
|
// Inflation area (1..252): green/yellow -> orange -> red with increasing alpha.
|
|
const t = Math.max(0, Math.min(1, costValue / 252));
|
|
r = Math.round(60 + 195 * t);
|
|
g = Math.round(255 - 220 * t);
|
|
b = Math.round(40 * (1 - t));
|
|
a = Math.round(35 + 190 * Math.pow(t, 0.65));
|
|
}
|
|
|
|
pixels[pxIdx] = r;
|
|
pixels[pxIdx + 1] = g;
|
|
pixels[pxIdx + 2] = b;
|
|
pixels[pxIdx + 3] = a;
|
|
}
|
|
}
|
|
|
|
this.costMapCacheCtx.putImageData(imageData, 0, 0);
|
|
this.costMapCacheReady = true;
|
|
},
|
|
|
|
applyCostMapUpdate: function(costMapData, costmapUpdate) {
|
|
if (!costMapData || !costMapData.data || !costmapUpdate) {
|
|
return;
|
|
}
|
|
|
|
const patchWidth = Number(costmapUpdate.width) || 0;
|
|
const patchHeight = Number(costmapUpdate.height) || 0;
|
|
const patchX = Number(costmapUpdate.x) || 0;
|
|
const patchY = Number(costmapUpdate.y) || 0;
|
|
|
|
if (patchWidth <= 0 || patchHeight <= 0) {
|
|
return;
|
|
}
|
|
|
|
const patchBytes = this.decodeCostMapBase64(costmapUpdate.dataBase64 || costmapUpdate.data);
|
|
if (!patchBytes || patchBytes.length === 0) {
|
|
return;
|
|
}
|
|
|
|
const maxPatchCells = patchWidth * patchHeight;
|
|
const patchCellCount = Math.min(maxPatchCells, patchBytes.length);
|
|
let applied = 0;
|
|
|
|
for (let i = 0; i < patchCellCount; i++) {
|
|
const localX = i % patchWidth;
|
|
const localY = Math.floor(i / patchWidth);
|
|
const mapX = patchX + localX;
|
|
const mapY = patchY + localY;
|
|
|
|
if (mapX < 0 || mapX >= costMapData.width || mapY < 0 || mapY >= costMapData.height) {
|
|
continue;
|
|
}
|
|
|
|
const mapIdx = mapY * costMapData.width + mapX;
|
|
costMapData.data[mapIdx] = patchBytes[i];
|
|
applied++;
|
|
}
|
|
|
|
if (applied > 0) {
|
|
this.rebuildCostMapCache();
|
|
}
|
|
},
|
|
|
|
normalizeFrameId: function(frameId) {
|
|
return (frameId || '').toString().replace(/^\/+/, '').toLowerCase();
|
|
},
|
|
|
|
lockMapFromOdomTransform: function(odometry, xlocPose) {
|
|
const odomX = Number(odometry?.x);
|
|
const odomY = Number(odometry?.y);
|
|
const odomYaw = Number(odometry?.yaw);
|
|
const mapX = Number(xlocPose?.x);
|
|
const mapY = Number(xlocPose?.y);
|
|
const mapYaw = Number(xlocPose?.yaw);
|
|
|
|
const hasOdom = Number.isFinite(odomX) && Number.isFinite(odomY) && Number.isFinite(odomYaw);
|
|
const hasMapPose = Number.isFinite(mapX) && Number.isFinite(mapY) && Number.isFinite(mapYaw);
|
|
if (!hasOdom || !hasMapPose) {
|
|
return false;
|
|
}
|
|
|
|
// map<-odom = map<-base * inverse(odom<-base)
|
|
const c = Math.cos(mapYaw);
|
|
const s = Math.sin(mapYaw);
|
|
const tx = mapX - (c * odomX - s * odomY);
|
|
const ty = mapY - (s * odomX + c * odomY);
|
|
|
|
this.costMapMapFromOdomLock = {
|
|
tx: tx,
|
|
ty: ty,
|
|
yaw: mapYaw - odomYaw
|
|
};
|
|
return true;
|
|
},
|
|
|
|
transformPointFromOdomToMap: function(x, y) {
|
|
if (!this.costMapMapFromOdomLock) {
|
|
return { x: x, y: y };
|
|
}
|
|
|
|
const t = this.costMapMapFromOdomLock;
|
|
const c = Math.cos(t.yaw);
|
|
const s = Math.sin(t.yaw);
|
|
return {
|
|
x: t.tx + c * x - s * y,
|
|
y: t.ty + s * x + c * y
|
|
};
|
|
},
|
|
|
|
updateCostMapPoseFromOdometry: function(odometry, localizationActive, xlocPose, matchingScore) {
|
|
if (!this.costMapData) {
|
|
return;
|
|
}
|
|
|
|
const odom = odometry || null;
|
|
const odomX = Number(odom?.x);
|
|
const odomY = Number(odom?.y);
|
|
const odomYaw = Number(odom?.yaw);
|
|
const hasOdom = Number.isFinite(odomX) && Number.isFinite(odomY);
|
|
|
|
const frameId = this.normalizeFrameId(this.costMapData.frameId);
|
|
const isOdomFrame = frameId === 'odom';
|
|
const hasValidLocalization = Number.isFinite(matchingScore) && matchingScore !== -1;
|
|
const localizationJustSucceeded = hasValidLocalization && this.costMapLastMatchingScore === -1;
|
|
|
|
// Lock costmap orientation and map<-odom transform when localization starts.
|
|
if (localizationActive && !this.costMapLocalizationActive) {
|
|
if (isOdomFrame && hasValidLocalization) {
|
|
this.lockMapFromOdomTransform(odom, xlocPose);
|
|
}
|
|
}
|
|
|
|
// Update costmap yaw to match robot yaw when localization successfully calculates (matching score != -1)
|
|
if (hasValidLocalization && (localizationJustSucceeded || localizationActive && !this.costMapLocalizationActive)) {
|
|
const oldYaw = this.costMapFixedYaw;
|
|
let baseYaw = 0;
|
|
if (xlocPose && Number.isFinite(Number(xlocPose.yaw))) {
|
|
baseYaw = Number(xlocPose.yaw);
|
|
} else if (Number.isFinite(odomYaw)) {
|
|
baseYaw = odomYaw;
|
|
} else {
|
|
baseYaw = Number(this.costMapData.originTheta) || 0;
|
|
}
|
|
|
|
// Rotate costmap 45° clockwise to align inflation with wall boundaries
|
|
const offsetDegrees = 190; // Negative = clockwise
|
|
const offsetRadians = offsetDegrees * Math.PI / 180;
|
|
this.costMapFixedYaw = baseYaw + offsetRadians;
|
|
|
|
if (Number.isFinite(this.costMapFixedYaw)) {
|
|
const baseYawDeg = baseYaw * 180 / Math.PI;
|
|
const finalYawDeg = this.costMapFixedYaw * 180 / Math.PI;
|
|
const oldYawDeg = Number.isFinite(oldYaw) ? oldYaw * 180 / Math.PI : null;
|
|
const offsetLabel = offsetDegrees > 0 ? `+${offsetDegrees}° CCW` : `${Math.abs(offsetDegrees)}° CW`;
|
|
console.log(`[Costmap] Locked yaw: Robot=${baseYawDeg.toFixed(1)}° ${offsetLabel} = ${finalYawDeg.toFixed(1)}° (matching score: ${matchingScore.toFixed(2)})` +
|
|
(oldYawDeg !== null ? `, was ${oldYawDeg.toFixed(1)}°` : ''));
|
|
}
|
|
}
|
|
|
|
this.costMapLocalizationActive = localizationActive;
|
|
this.costMapLastMatchingScore = matchingScore;
|
|
|
|
if (!Number.isFinite(this.costMapFixedYaw)) {
|
|
this.costMapFixedYaw = Number(this.costMapData.originTheta) || 0;
|
|
}
|
|
|
|
if (isOdomFrame) {
|
|
if (!this.costMapMapFromOdomLock && hasValidLocalization) {
|
|
this.lockMapFromOdomTransform(odom, xlocPose);
|
|
}
|
|
|
|
const transformed = this.transformPointFromOdomToMap(
|
|
Number(this.costMapData.originX) || 0,
|
|
Number(this.costMapData.originY) || 0
|
|
);
|
|
this.costMapData.poseX = transformed.x;
|
|
this.costMapData.poseY = transformed.y;
|
|
} else if (hasOdom && this.costMapPoseAnchor && this.costMapOdomAnchor) {
|
|
// Fallback for non-odom frame sources.
|
|
this.costMapData.poseX = this.costMapPoseAnchor.x + (odomX - this.costMapOdomAnchor.x);
|
|
this.costMapData.poseY = this.costMapPoseAnchor.y + (odomY - this.costMapOdomAnchor.y);
|
|
} else {
|
|
this.costMapData.poseX = Number(this.costMapData.originX) || 0;
|
|
this.costMapData.poseY = Number(this.costMapData.originY) || 0;
|
|
}
|
|
|
|
this.costMapData.fixedYaw = this.costMapFixedYaw;
|
|
},
|
|
|
|
drawCostMap: function() {
|
|
if (!this.showCostMap || !this.costMapData || !this.costMapData.data || !this.costMapCacheReady) {
|
|
return;
|
|
}
|
|
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
const { width, height, resolution, poseX, poseY, fixedYaw } = this.costMapData;
|
|
|
|
// After 90° CW rotation, dimensions are swapped in the cache canvas
|
|
const mapWidthPixels = height * resolution * this.scale;
|
|
const mapHeightPixels = width * resolution * this.scale;
|
|
|
|
// Calculate angle difference between costmap and robot
|
|
if (this.robotPose && Number.isFinite(this.robotPose.yaw) && Number.isFinite(fixedYaw)) {
|
|
const angleDiffRad = fixedYaw - this.robotPose.yaw;
|
|
const angleDiffDeg = angleDiffRad * 180 / Math.PI;
|
|
|
|
// Log once every 2 seconds to avoid spam
|
|
if (!this._lastCostMapAngleLog || Date.now() - this._lastCostMapAngleLog > 2000) {
|
|
console.log(`[Costmap] Offset from robot: ${angleDiffDeg.toFixed(1)}° (Robot: ${(this.robotPose.yaw * 180 / Math.PI).toFixed(1)}°, Costmap: ${(fixedYaw * 180 / Math.PI).toFixed(1)}°)`);
|
|
this._lastCostMapAngleLog = Date.now();
|
|
}
|
|
}
|
|
|
|
this.ctx.save();
|
|
this.ctx.globalAlpha = 0.75;
|
|
|
|
// Use robot position as center of rotation (robot should be at center of costmap)
|
|
let centerX, centerY;
|
|
if (this.robotPose) {
|
|
// Robot is the center of costmap
|
|
centerX = mapFrameX + (this.robotPose.x * this.scale);
|
|
centerY = mapFrameY - (this.robotPose.y * this.scale);
|
|
} else {
|
|
// Fallback to costmap origin + half dimensions
|
|
const halfWidthMeters = (height * resolution) / 2; // After 90° rotation
|
|
const halfHeightMeters = (width * resolution) / 2;
|
|
centerX = mapFrameX + ((poseX + halfWidthMeters) * this.scale);
|
|
centerY = mapFrameY - ((poseY + halfHeightMeters) * this.scale);
|
|
}
|
|
|
|
// Translate to center (robot position), rotate, then draw costmap centered
|
|
this.ctx.translate(centerX, centerY);
|
|
this.ctx.rotate(-(fixedYaw || 0));
|
|
|
|
// Translate costmap in robot frame: X=forward, Y=left
|
|
const xOffsetMeters = -0.285; // Forward offset
|
|
const yOffsetMeters = -0.707; // Left offset
|
|
this.ctx.translate(xOffsetMeters * this.scale, -yOffsetMeters * this.scale);
|
|
|
|
this.ctx.scale(1, -1);
|
|
|
|
// Draw costmap centered at origin (robot at center + offsets)
|
|
this.ctx.drawImage(this.costMapCacheCanvas, -mapWidthPixels / 2, -mapHeightPixels / 2, mapWidthPixels, mapHeightPixels);
|
|
|
|
// Draw costmap frame so its boundary is always visible.
|
|
this.ctx.globalAlpha = 1.0;
|
|
this.ctx.strokeStyle = 'rgba(0, 188, 212, 0.95)';
|
|
this.ctx.lineWidth = 2;
|
|
this.ctx.strokeRect(-mapWidthPixels / 2, -mapHeightPixels / 2, mapWidthPixels, mapHeightPixels);
|
|
|
|
this.ctx.restore();
|
|
},
|
|
|
|
/**
|
|
* Get LIDAR transform relative to base_link (from XlocClient.cs static transforms)
|
|
*/
|
|
getLidarTransform: function(frameId) {
|
|
// Defaults to identity if unknown
|
|
const defaultTransform = { x: 0, y: 0, yaw: 0 };
|
|
switch (frameId) {
|
|
case 'scan_1': {
|
|
// base_link -> scan_1
|
|
return {
|
|
x: 0.2985,
|
|
y: 0.0,
|
|
yaw: this.quaternionToYaw(0.0, 0.0, 0.0, 1.0)
|
|
};
|
|
}
|
|
case 'scan_2': {
|
|
// base_link -> scan_2
|
|
return {
|
|
x: -0.2985,
|
|
y: -0.0,
|
|
yaw: this.quaternionToYaw(0.0, 0.0, 1.0, 0.0)
|
|
};
|
|
}
|
|
case 'scan_3': {
|
|
// base_link -> scan_3
|
|
return {
|
|
x: 0.707,
|
|
y: 0.2825,
|
|
yaw: this.quaternionToYaw(0.0, 0.0, 0.3826834, 0.9238795)
|
|
};
|
|
}
|
|
default:
|
|
return defaultTransform;
|
|
}
|
|
},
|
|
|
|
/**
|
|
* Convert quaternion to yaw (radians) for Z rotation only.
|
|
*/
|
|
quaternionToYaw: function(qx, qy, qz, qw) {
|
|
const sinyCosp = 2.0 * (qw * qz + qx * qy);
|
|
const cosyCosp = 1.0 - 2.0 * (qy * qy + qz * qz);
|
|
return Math.atan2(sinyCosp, cosyCosp);
|
|
},
|
|
|
|
/**
|
|
* Draw a single LIDAR scan
|
|
* LIDAR points are transformed from robot frame -> map frame -> canvas
|
|
*/
|
|
drawSingleLidarScan: function(scanData, color, lidarTransform, renderMode) {
|
|
if (!scanData || !scanData.points || scanData.points.length === 0) return;
|
|
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
// Get robot pose in MAP coordinates for transformation
|
|
const robotX = this.robotPose ? this.robotPose.x : 0;
|
|
const robotY = this.robotPose ? this.robotPose.y : 0;
|
|
const robotYaw = this.robotPose ? this.robotPose.yaw : 0;
|
|
|
|
this.ctx.fillStyle = color;
|
|
|
|
const lidar = lidarTransform || { x: 0, y: 0, yaw: 0 };
|
|
const cosLidarYaw = Math.cos(lidar.yaw);
|
|
const sinLidarYaw = Math.sin(lidar.yaw);
|
|
|
|
// Backend now handles sampling based on mode: minimal = 120 points, full = all points
|
|
// Frontend just draws all points received from backend
|
|
for (let i = 0; i < scanData.points.length; i++) {
|
|
const point = scanData.points[i];
|
|
let pointX, pointY;
|
|
|
|
// Check if point has x,y (cartesian) or angle,range (polar)
|
|
if (point.x !== undefined && point.y !== undefined) {
|
|
// Already in cartesian coordinates (robot frame)
|
|
pointX = point.x;
|
|
pointY = point.y;
|
|
} else if (point.angle !== undefined && point.range !== undefined) {
|
|
// Convert from polar to cartesian (robot frame)
|
|
// LIDAR convention: angle 0 = forward (positive X)
|
|
pointX = point.range * Math.cos(point.angle);
|
|
pointY = point.range * Math.sin(point.angle);
|
|
} else {
|
|
continue; // Skip invalid point
|
|
}
|
|
|
|
// Transform from LIDAR frame -> robot frame
|
|
const robotPointX = lidar.x + (pointX * cosLidarYaw - pointY * sinLidarYaw);
|
|
const robotPointY = lidar.y + (pointX * sinLidarYaw + pointY * cosLidarYaw);
|
|
|
|
// Transform from robot frame to MAP frame
|
|
const cosYaw = Math.cos(robotYaw);
|
|
const sinYaw = Math.sin(robotYaw);
|
|
const mapX = robotX + (robotPointX * cosYaw - robotPointY * sinYaw);
|
|
const mapY = robotY + (robotPointX * sinYaw + robotPointY * cosYaw);
|
|
|
|
// Convert MAP coordinates to canvas coordinates
|
|
// Same transform as map: mapFrameX + mapX * scale, mapFrameY - mapY * scale (flip Y)
|
|
const canvasX = mapFrameX + (mapX * this.scale);
|
|
const canvasY = mapFrameY - (mapY * this.scale);
|
|
|
|
// Draw point (larger size: 3px radius)
|
|
this.ctx.beginPath();
|
|
this.ctx.arc(canvasX, canvasY, 3, 0, 2 * Math.PI);
|
|
this.ctx.fill();
|
|
}
|
|
},
|
|
|
|
/**
|
|
* Draw map frame coordinate axes (fixed at origin)
|
|
*/
|
|
drawCoordinateAxes: function() {
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
const axisLength = 60;
|
|
|
|
// Origin marker
|
|
this.ctx.fillStyle = '#FF9800';
|
|
this.ctx.strokeStyle = '#F57C00';
|
|
this.ctx.lineWidth = 2;
|
|
this.ctx.beginPath();
|
|
this.ctx.arc(mapFrameX, mapFrameY, 6, 0, 2 * Math.PI);
|
|
this.ctx.fill();
|
|
this.ctx.stroke();
|
|
|
|
// X axis (red)
|
|
this.ctx.strokeStyle = '#F44336';
|
|
this.ctx.lineWidth = 3;
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(mapFrameX, mapFrameY);
|
|
this.ctx.lineTo(mapFrameX + axisLength, mapFrameY);
|
|
this.ctx.stroke();
|
|
|
|
// X arrow
|
|
this.ctx.fillStyle = '#F44336';
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(mapFrameX + axisLength, mapFrameY);
|
|
this.ctx.lineTo(mapFrameX + axisLength - 8, mapFrameY - 5);
|
|
this.ctx.lineTo(mapFrameX + axisLength - 8, mapFrameY + 5);
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
|
|
// X label
|
|
this.ctx.fillStyle = '#F44336';
|
|
this.ctx.font = 'bold 12px Arial';
|
|
this.ctx.fillText('X', mapFrameX + axisLength + 8, mapFrameY + 5);
|
|
|
|
// Y axis (green)
|
|
this.ctx.strokeStyle = '#4CAF50';
|
|
this.ctx.lineWidth = 3;
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(mapFrameX, mapFrameY);
|
|
this.ctx.lineTo(mapFrameX, mapFrameY - axisLength);
|
|
this.ctx.stroke();
|
|
|
|
// Y arrow
|
|
this.ctx.fillStyle = '#4CAF50';
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(mapFrameX, mapFrameY - axisLength);
|
|
this.ctx.lineTo(mapFrameX - 5, mapFrameY - axisLength + 8);
|
|
this.ctx.lineTo(mapFrameX + 5, mapFrameY - axisLength + 8);
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
|
|
// Y label
|
|
this.ctx.fillStyle = '#4CAF50';
|
|
this.ctx.font = 'bold 12px Arial';
|
|
this.ctx.fillText('Y', mapFrameX + 8, mapFrameY - axisLength - 5);
|
|
},
|
|
drawWorldCoordinateAxes: function() {
|
|
if (!this.lockedMapOrigin) return; // Only draw if map origin is known
|
|
|
|
const { mapFrameX, mapFrameY } = this.getFramePositions();
|
|
const originXY = this.getMapOriginXY();
|
|
|
|
// Calculate world (0,0) position in canvas coordinates
|
|
// Map origin is the world position of map (0,0), so world (0,0) = map (0,0) - origin
|
|
// In canvas: worldX = mapFrameX + (-originX) * scale, worldY = mapFrameY - (-originY) * scale
|
|
const worldOriginX = mapFrameX - (originXY.x * this.scale);
|
|
const worldOriginY = mapFrameY + (originXY.y * this.scale);
|
|
|
|
const axisLength = 50;
|
|
|
|
// Origin marker (blue for world frame)
|
|
this.ctx.fillStyle = '#2196F3';
|
|
this.ctx.strokeStyle = '#1976D2';
|
|
this.ctx.lineWidth = 2;
|
|
this.ctx.beginPath();
|
|
this.ctx.arc(worldOriginX, worldOriginY, 5, 0, 2 * Math.PI);
|
|
this.ctx.fill();
|
|
this.ctx.stroke();
|
|
|
|
// X axis (red, dashed for world frame)
|
|
this.ctx.strokeStyle = '#F44336';
|
|
this.ctx.lineWidth = 2.5;
|
|
this.ctx.setLineDash([5, 5]);
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(worldOriginX, worldOriginY);
|
|
this.ctx.lineTo(worldOriginX + axisLength, worldOriginY);
|
|
this.ctx.stroke();
|
|
this.ctx.setLineDash([]);
|
|
|
|
// X arrow
|
|
this.ctx.fillStyle = '#F44336';
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(worldOriginX + axisLength, worldOriginY);
|
|
this.ctx.lineTo(worldOriginX + axisLength - 8, worldOriginY - 5);
|
|
this.ctx.lineTo(worldOriginX + axisLength - 8, worldOriginY + 5);
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
|
|
// X label
|
|
this.ctx.fillStyle = '#F44336';
|
|
this.ctx.font = 'bold 11px Arial';
|
|
this.ctx.fillText('X_world', worldOriginX + axisLength + 8, worldOriginY + 5);
|
|
|
|
// Y axis (green, dashed for world frame)
|
|
this.ctx.strokeStyle = '#4CAF50';
|
|
this.ctx.lineWidth = 2.5;
|
|
this.ctx.setLineDash([5, 5]);
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(worldOriginX, worldOriginY);
|
|
this.ctx.lineTo(worldOriginX, worldOriginY - axisLength);
|
|
this.ctx.stroke();
|
|
this.ctx.setLineDash([]);
|
|
|
|
// Y arrow
|
|
this.ctx.fillStyle = '#4CAF50';
|
|
this.ctx.beginPath();
|
|
this.ctx.moveTo(worldOriginX, worldOriginY - axisLength);
|
|
this.ctx.lineTo(worldOriginX - 5, worldOriginY - axisLength + 8);
|
|
this.ctx.lineTo(worldOriginX + 5, worldOriginY - axisLength + 8);
|
|
this.ctx.closePath();
|
|
this.ctx.fill();
|
|
|
|
// Y label
|
|
this.ctx.fillStyle = '#4CAF50';
|
|
this.ctx.font = 'bold 11px Arial';
|
|
this.ctx.fillText('Y_world', worldOriginX + 8, worldOriginY - axisLength - 5);
|
|
},
|
|
/**
|
|
* Set zoom level (scale)
|
|
*/
|
|
setZoom: function(newScale) {
|
|
this.scale = Math.max(1, Math.min(100, newScale)); // Clamp between 1 and 100
|
|
this.render();
|
|
},
|
|
|
|
/**
|
|
* Zoom in (increase scale by 20%)
|
|
*/
|
|
zoomIn: function() {
|
|
const newScale = this.scale * 1.2;
|
|
if (newScale <= 100) {
|
|
this.scale = newScale;
|
|
this.render();
|
|
}
|
|
},
|
|
|
|
/**
|
|
* Zoom out (decrease scale by 20%)
|
|
*/
|
|
zoomOut: function() {
|
|
const newScale = this.scale / 1.2;
|
|
if (newScale >= 1) {
|
|
this.scale = newScale;
|
|
this.render();
|
|
}
|
|
},
|
|
|
|
/**
|
|
* Reset zoom to default
|
|
*/
|
|
resetZoom: function() {
|
|
this.scale = 20;
|
|
this.render();
|
|
},
|
|
|
|
/**
|
|
* Reset view to default (zoom + pan)
|
|
*/
|
|
resetView: function() {
|
|
this.offsetX = 0;
|
|
this.offsetY = 0;
|
|
this.scale = 20;
|
|
this.render();
|
|
},
|
|
|
|
/**
|
|
* Reset locked values (call when loading a new map)
|
|
*/
|
|
resetLocks: function() {
|
|
this.lockedMapOrigin = null;
|
|
this.lockedOffsetX = null;
|
|
this.lockedOffsetY = null;
|
|
this.costMapData = null;
|
|
this.costMapCacheReady = false;
|
|
this.costMapPoseAnchor = null;
|
|
this.costMapOdomAnchor = null;
|
|
this.costMapFixedYaw = null;
|
|
this.costMapMapFromOdomLock = null;
|
|
this.costMapLocalizationActive = false;
|
|
this.costMapLastMatchingScore = -1;
|
|
},
|
|
|
|
/**
|
|
* Get current view state (for debugging/verification)
|
|
* Returns JSON string for C# deserialization
|
|
*/
|
|
getViewState: function() {
|
|
const state = {
|
|
offsetX: this.offsetX,
|
|
offsetY: this.offsetY,
|
|
scale: this.scale,
|
|
lockedMapOrigin: this.lockedMapOrigin,
|
|
gridMapOrigin: this.gridMap?.origin
|
|
};
|
|
return JSON.stringify(state);
|
|
},
|
|
|
|
/**
|
|
* Set view offsets (for restoring after verification)
|
|
*/
|
|
setViewOffsets: function(offsetX, offsetY) {
|
|
this.offsetX = Number(offsetX);
|
|
this.offsetY = Number(offsetY);
|
|
|
|
// CRITICAL: If locked offsets exist, update them to match (for consistency)
|
|
if (this.lockedOffsetX !== null && this.lockedOffsetY !== null) {
|
|
this.lockedOffsetX = this.offsetX;
|
|
this.lockedOffsetY = this.offsetY;
|
|
}
|
|
|
|
this.requestRender();
|
|
}
|
|
};
|
|
|
|
// Global wrapper functions for JSInterop debugging
|
|
function debugSetGlobalPathData(pathPoints) {
|
|
console.log('🎯 Global wrapper function called: debugSetGlobalPathData');
|
|
console.log(' Received:', pathPoints);
|
|
if (window.xlocMapRenderer) {
|
|
console.log(' ✅ xlocMapRenderer exists, calling setGlobalPathData...');
|
|
return window.xlocMapRenderer.setGlobalPathData(pathPoints);
|
|
} else {
|
|
console.error(' ❌ xlocMapRenderer NOT FOUND!');
|
|
return null;
|
|
}
|
|
}
|
|
|
|
|