136 lines
3.7 KiB
JavaScript
136 lines
3.7 KiB
JavaScript
// Tim781S LiDAR Visualization JavaScript
|
|
|
|
let canvas, ctx;
|
|
let centerX, centerY;
|
|
let scale = 20; // pixels per meter
|
|
let maxDisplay = 20; // max display range in meters
|
|
|
|
function initLidarCanvas() {
|
|
canvas = document.getElementById('lidarCanvas');
|
|
if (!canvas) {
|
|
console.error('LiDAR canvas not found');
|
|
return;
|
|
}
|
|
|
|
ctx = canvas.getContext('2d');
|
|
centerX = canvas.width / 2;
|
|
centerY = canvas.height / 2;
|
|
|
|
// Draw initial grid
|
|
drawGrid();
|
|
|
|
console.log('LiDAR canvas initialized');
|
|
}
|
|
|
|
function drawGrid() {
|
|
if (!ctx) return;
|
|
|
|
ctx.clearRect(0, 0, canvas.width, canvas.height);
|
|
|
|
// Draw background
|
|
ctx.fillStyle = '#0a0a0a';
|
|
ctx.fillRect(0, 0, canvas.width, canvas.height);
|
|
|
|
// Draw grid circles (range rings)
|
|
ctx.strokeStyle = '#2a2a2a';
|
|
ctx.lineWidth = 1;
|
|
|
|
for (let r = 5; r <= maxDisplay; r += 5) {
|
|
ctx.beginPath();
|
|
ctx.arc(centerX, centerY, r * scale, 0, 2 * Math.PI);
|
|
ctx.stroke();
|
|
|
|
// Draw range labels
|
|
ctx.fillStyle = '#666';
|
|
ctx.font = '12px monospace';
|
|
ctx.fillText(r + 'm', centerX + 5, centerY - r * scale + 15);
|
|
}
|
|
|
|
// Draw axes
|
|
ctx.strokeStyle = '#444';
|
|
ctx.lineWidth = 2;
|
|
|
|
// X axis
|
|
ctx.beginPath();
|
|
ctx.moveTo(0, centerY);
|
|
ctx.lineTo(canvas.width, centerY);
|
|
ctx.stroke();
|
|
|
|
// Y axis
|
|
ctx.beginPath();
|
|
ctx.moveTo(centerX, 0);
|
|
ctx.lineTo(centerX, canvas.height);
|
|
ctx.stroke();
|
|
|
|
// Draw robot position (center circle)
|
|
ctx.fillStyle = '#00ff88';
|
|
ctx.beginPath();
|
|
ctx.arc(centerX, centerY, 5, 0, 2 * Math.PI);
|
|
ctx.fill();
|
|
|
|
// Draw forward direction indicator
|
|
ctx.strokeStyle = '#00ff88';
|
|
ctx.lineWidth = 3;
|
|
ctx.beginPath();
|
|
ctx.moveTo(centerX, centerY);
|
|
ctx.lineTo(centerX, centerY - 30);
|
|
ctx.stroke();
|
|
}
|
|
|
|
function drawLidarScan(points) {
|
|
if (!ctx || !Array.isArray(points)) return;
|
|
|
|
// Redraw grid first
|
|
drawGrid();
|
|
|
|
// Draw scan points
|
|
points.forEach(point => {
|
|
const angle = point.angle;
|
|
const range = point.range;
|
|
|
|
// Convert polar to Cartesian (LiDAR coordinate frame)
|
|
// In LiDAR frame: 0° = forward (up on screen)
|
|
// We need to rotate 90° so forward matches up on screen
|
|
const x = centerX + (range * scale * Math.sin(angle));
|
|
const y = centerY - (range * scale * Math.cos(angle));
|
|
|
|
// Color based on distance (closer = brighter green, farther = darker)
|
|
const intensity = 1.0 - (range / maxDisplay);
|
|
const r = Math.floor(0 * intensity);
|
|
const g = Math.floor(255 * intensity);
|
|
const b = Math.floor(136 * intensity);
|
|
|
|
// Draw point
|
|
ctx.fillStyle = `rgb(${r}, ${g}, ${b})`;
|
|
ctx.beginPath();
|
|
ctx.arc(x, y, 2, 0, 2 * Math.PI);
|
|
ctx.fill();
|
|
});
|
|
|
|
// Draw connection lines (optional - creates solid appearance)
|
|
if (points.length > 1) {
|
|
ctx.strokeStyle = 'rgba(0, 255, 136, 0.3)';
|
|
ctx.lineWidth = 1;
|
|
ctx.beginPath();
|
|
|
|
points.forEach((point, index) => {
|
|
const angle = point.angle;
|
|
const range = point.range;
|
|
const x = centerX + (range * scale * Math.sin(angle));
|
|
const y = centerY - (range * scale * Math.cos(angle));
|
|
|
|
if (index === 0) {
|
|
ctx.moveTo(x, y);
|
|
} else {
|
|
ctx.lineTo(x, y);
|
|
}
|
|
});
|
|
|
|
ctx.stroke();
|
|
}
|
|
}
|
|
|
|
// Make functions available globally
|
|
window.initLidarCanvas = initLidarCanvas;
|
|
window.drawLidarScan = drawLidarScan;
|