Files
lms-web-fe/public/slam-viewer/index.html
T
2026-07-15 13:41:00 +09:00

1343 lines
44 KiB
HTML

<!DOCTYPE html>
<html lang="ko">
<head>
<meta charset="UTF-8" />
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
<title>TurtleBot3 World Viewer</title>
<style>
html, body { margin:0; width:100%; height:100%; overflow:hidden; background:#9f9f9f; }
canvas { display:block; }
#miniMap {
position:absolute;
right:16px;
bottom:16px;
width:260px;
height:220px;
z-index:10;
background:#62736f;
border:1px solid rgba(0,0,0,.45);
pointer-events:none;
}
#miniMap.hidden {
display:none;
}
#miniMapToggle {
position:absolute;
right:16px;
bottom:236px;
z-index:11;
min-height:28px;
padding:0 10px;
border:1px solid rgba(255,255,255,.22);
background:rgba(17,24,39,.42);
color:rgba(255,255,255,.88);
font:12px sans-serif;
font-weight:800;
cursor:pointer;
backdrop-filter:blur(10px);
box-shadow:0 10px 26px rgba(0,0,0,.16);
transition:background .15s ease, border-color .15s ease, color .15s ease;
}
#miniMapToggle:hover {
background:rgba(17,24,39,.58);
border-color:rgba(255,255,255,.34);
color:#fff;
}
#miniMap.hidden + #miniMapToggle {
bottom:16px;
}
#topRightControls {
position:absolute;
right:16px;
top:16px;
z-index:10;
display:flex;
gap:7px;
align-items:flex-start;
}
#keyGuide {
position:absolute;
left:16px;
bottom:16px;
z-index:10;
padding:7px 9px;
border:1px solid rgba(255,255,255,.24);
background:rgba(17,24,39,.34);
color:rgba(255,255,255,.82);
font:11px/1.35 sans-serif;
pointer-events:none;
backdrop-filter:blur(8px);
box-shadow:0 10px 28px rgba(0,0,0,.16);
}
#downloadWrap {
position:relative;
}
#downloadMap,
#pathToggle,
#goalToggle,
#cameraPresetSelect {
min-height:32px;
border:1px solid rgba(255,255,255,.22);
background:rgba(17,24,39,.42);
color:rgba(255,255,255,.88);
font:12px sans-serif;
font-weight:700;
cursor:pointer;
backdrop-filter:blur(10px);
box-shadow:0 10px 26px rgba(0,0,0,.16);
transition:background .15s ease, border-color .15s ease, color .15s ease;
}
#downloadMap,
#pathToggle,
#goalToggle {
padding:0 11px;
}
#downloadMap {
display:inline-flex;
align-items:center;
gap:6px;
}
#cameraPresetSelect {
min-width:86px;
padding:0 24px 0 9px;
outline:none;
appearance:none;
background-image:linear-gradient(45deg, transparent 50%, rgba(255,255,255,.7) 50%), linear-gradient(135deg, rgba(255,255,255,.7) 50%, transparent 50%);
background-position:calc(100% - 13px) 14px, calc(100% - 8px) 14px;
background-size:5px 5px, 5px 5px;
background-repeat:no-repeat;
}
#downloadMap:hover,
#pathToggle:hover,
#goalToggle:hover,
#cameraPresetSelect:hover {
background-color:rgba(17,24,39,.58);
border-color:rgba(255,255,255,.34);
color:#fff;
}
#downloadMap:disabled {
opacity:.45;
cursor:not-allowed;
}
#pathToggle.active {
background:rgba(37,99,235,.62);
border-color:rgba(147,197,253,.55);
color:#fff;
}
#goalToggle.active {
background:rgba(22,163,74,.62);
border-color:rgba(134,239,172,.55);
color:#fff;
}
#downloadMenu {
position:absolute;
right:0;
top:38px;
min-width:104px;
display:none;
overflow:hidden;
border:1px solid rgba(255,255,255,.22);
background:rgba(17,24,39,.72);
backdrop-filter:blur(10px);
box-shadow:0 14px 32px rgba(0,0,0,.22);
}
#downloadMenu.open { display:block; }
#downloadMenu button {
display:block;
width:100%;
padding:8px 11px;
border:0;
background:transparent;
color:rgba(255,255,255,.82);
text-align:left;
font:12px sans-serif;
font-weight:700;
cursor:pointer;
}
#downloadMenu button:hover {
background:rgba(255,255,255,.1);
color:#fff;
}
</style>
</head>
<body>
<div id="topRightControls">
<div id="downloadWrap">
<button id="downloadMap" title="맵 다운로드" aria-label="맵 다운로드" disabled><span aria-hidden="true">▣</span><span>Map</span><span aria-hidden="true">▾</span></button>
<div id="downloadMenu">
<button data-format="yaml">YAML</button>
<button data-format="pgm">PGM</button>
<button data-format="pcd">PCD</button>
</div>
</div>
<button id="pathToggle" title="로봇 경로 표시" aria-label="로봇 경로 표시">Path</button>
<button id="goalToggle" title="목표 위치 지정" aria-label="목표 위치 지정">Goal</button>
<select id="cameraPresetSelect" title="카메라 시점" aria-label="카메라 시점">
<option value="default">Default</option>
<option value="top">Top</option>
<option value="follow">Follow</option>
</select>
</div>
<canvas id="miniMap" width="260" height="220"></canvas>
<button id="miniMapToggle" title="미니맵 접기/펼치기" aria-label="미니맵 접기">▾</button>
<div id="keyGuide">
<div>i 전진 · , 후진 · j/l 회전 · k 정지</div>
</div>
<script type="importmap">
{
"imports": {
"three": "/vendor/three/build/three.module.js",
"three/addons/": "/vendor/three/examples/jsm/"
}
}
</script>
<script type="module">
import * as THREE from 'three';
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
import { ColladaLoader } from 'three/addons/loaders/ColladaLoader.js';
import { STLLoader } from 'three/addons/loaders/STLLoader.js';
const MODEL_ROOT = './turtlebot3_world_model/turtlebot3_world';
const MODEL_SDF = `${MODEL_ROOT}/model.sdf`;
const miniMap = document.getElementById('miniMap');
const miniCtx = miniMap.getContext('2d');
const miniMapToggle = document.getElementById('miniMapToggle');
const downloadMapButton = document.getElementById('downloadMap');
const downloadMenu = document.getElementById('downloadMenu');
const pathToggleButton = document.getElementById('pathToggle');
const goalToggleButton = document.getElementById('goalToggle');
const cameraPresetSelect = document.getElementById('cameraPresetSelect');
const params = new URLSearchParams(location.search);
const DIRECT_BRIDGE = params.get('directBridge') === '1';
const BACKEND_URL = params.get('backend') || params.get('bridgeHttp') || 'http://localhost:8765';
const BACKEND_WS = params.get('bridgeWs')
|| BACKEND_URL.replace(/^http:/, 'ws:').replace(/^https:/, 'wss:') + (DIRECT_BRIDGE ? '/ws/state' : '/ws/slam');
function focusViewer() {
document.activeElement?.blur?.();
renderer.domElement.focus();
}
const scene = new THREE.Scene();
scene.background = new THREE.Color(0x9f9f9f);
const camera = new THREE.PerspectiveCamera(45, innerWidth / innerHeight, 0.05, 1000);
// Three.js OrbitControls 기본 조작이 자연스럽게 동작하도록 사선 시점에서 시작합니다.
camera.position.set(4, 6, 5);
camera.up.set(0, 1, 0);
const renderer = new THREE.WebGLRenderer({ antialias:true });
renderer.setSize(innerWidth, innerHeight);
renderer.setPixelRatio(Math.min(devicePixelRatio, 1.25));
renderer.shadowMap.enabled = true;
renderer.domElement.tabIndex = 0;
document.body.appendChild(renderer.domElement);
const controls = new OrbitControls(camera, renderer.domElement);
controls.mouseButtons = {
LEFT: THREE.MOUSE.PAN,
MIDDLE: THREE.MOUSE.ROTATE,
RIGHT: THREE.MOUSE.DOLLY,
};
renderer.domElement.addEventListener('contextmenu', (e) => e.preventDefault());
controls.enableDamping = true;
controls.dampingFactor = 0.08;
controls.target.set(0, 0, 0);
camera.lookAt(controls.target);
scene.add(new THREE.AmbientLight(0xffffff, .65));
const sun = new THREE.DirectionalLight(0xffffff, .85);
sun.position.set(3, 6, 4);
sun.castShadow = true;
sun.shadow.mapSize.set(512, 512);
scene.add(sun);
// Three.js 화면용 바닥/격자입니다. 실제 SDF 모델과 별개입니다.
const floor = new THREE.Mesh(
new THREE.PlaneGeometry(42, 42),
new THREE.MeshLambertMaterial({ color: 0xd0d0d0 })
);
floor.rotation.x = -Math.PI / 2;
floor.position.y = -0.01;
floor.receiveShadow = true;
scene.add(floor);
scene.add(new THREE.GridHelper(14, 14, 0x333333, 0x555555));
// SDF/Gazebo는 Z-up, Three.js는 Y-up입니다.
// 그래서 모든 SDF 객체를 이 그룹 안에 원래 SDF 좌표 그대로 넣고,
// 그룹 전체만 -90도 회전시켜 Three.js 좌표계로 변환합니다.
const sdfRoot = new THREE.Group();
sdfRoot.rotation.x = -Math.PI / 2;
scene.add(sdfRoot);
let robotModel = null;
let laserGroup = null;
let slamSocket = null;
let latestSlamState = null;
let currentRobotPose = { x:-2.0, y:-0.5, z:0.01, roll:0, pitch:0, yaw:0 };
let targetRobotPose = { ...currentRobotPose };
let hasRobotPose = false;
let latestMiniMapGrid = null;
let miniMapBitmap = null;
let latestScanMetadata = null;
let latestScanRanges = null;
let pathVisible = false;
const robotPathPoints = [];
let robotPathLine = null;
let activeCameraPreset = 'default';
let navigationGoalMode = false;
let navigationGoalMarker = null;
const navigationRaycaster = new THREE.Raycaster();
const navigationGroundPlane = new THREE.Plane(new THREE.Vector3(0, 1, 0), 0);
function addWorldAxis(length = 1000) {
const axis = new THREE.Group();
axis.position.z = 0.02;
const axes = [
{ color:0xff0000, points:[[0,0,0], [length,0,0]] },
{ color:0x00aa00, points:[[0,0,0], [0,length,0]] },
{ color:0x0066ff, points:[[0,0,0], [0,0,length]] },
];
for (const item of axes) {
const geo = new THREE.BufferGeometry().setFromPoints(
item.points.map(([x, y, z]) => new THREE.Vector3(x, y, z))
);
axis.add(new THREE.Line(geo, new THREE.LineBasicMaterial({ color:item.color })));
}
sdfRoot.add(axis);
}
addWorldAxis();
function parseNums(text, count = 0) {
const nums = (text || '').trim().split(/\s+/).filter(Boolean).map(Number).filter(n => !Number.isNaN(n));
while (nums.length < count) nums.push(0);
return nums;
}
function parsePose(text) {
const v = parseNums(text, 6);
return { x:v[0], y:v[1], z:v[2], roll:v[3], pitch:v[4], yaw:v[5] };
}
function applySdfPose(obj, pose) {
obj.position.set(pose.x, pose.y, pose.z);
obj.rotation.set(pose.roll, pose.pitch, pose.yaw, 'XYZ');
}
function getFirst(el, tag) { return el?.getElementsByTagName(tag)?.[0] || null; }
function directChild(el, tag) { return Array.from(el.children || []).find(c => c.tagName === tag) || null; }
function getDirectText(el, tag, fallback = '') {
const n = directChild(el, tag);
return n?.textContent?.trim() || fallback;
}
const MODEL_ROOTS = {
turtlebot3_world: './turtlebot3_world_model/turtlebot3_world',
turtlebot3_common: './turtlebot3_world_model/turtlebot3_common',
};
function gazeboUriToPath(uri) {
return uri.replace(/^model:\/\/(\w+)\//, (_, name) =>
(MODEL_ROOTS[name] ?? `./turtlebot3_world_model/${name}`) + '/'
);
}
function parseScale(text) {
const s = parseNums(text, 3);
if (s.length >= 3 && s.every(n => Number.isFinite(n))) return new THREE.Vector3(s[0], s[1], s[2]);
return new THREE.Vector3(1,1,1);
}
function materialFromVisual(visual) {
let color = 0xdddddd;
const visualName = visual.getAttribute('name') || '';
const uriText = Array.from(visual.getElementsByTagName('uri')).map(n => n.textContent.trim()).join(' ');
const scriptName = Array.from(visual.getElementsByTagName('script')).map(s => getDirectText(s, 'name', '')).join(' ');
const combined = `${visualName} ${uriText} ${scriptName}`.toLowerCase();
if (combined.includes('wall')) color = 0x080808;
else if (combined.includes('hexagon') || combined.includes('green')) color = 0x00a000;
else if (combined.includes('cylinder') || combined.includes('white')) color = 0xf0f0f0;
const ambientNode = visual.getElementsByTagName('ambient')[0];
const ambient = ambientNode?.textContent?.trim();
if (ambient) {
const c = parseNums(ambient);
if (c.length >= 3) color = new THREE.Color(c[0], c[1], c[2]);
}
return new THREE.MeshLambertMaterial({ color });
}
function prepMesh(mesh, material) {
mesh.traverse(child => {
if (child.isMesh) {
child.material = material.clone();
child.castShadow = true;
child.receiveShadow = true;
}
});
}
const colladaLoader = new ColladaLoader();
if (colladaLoader.options) colladaLoader.options.convertUpAxis = false;
const stlLoader = new STLLoader();
async function addMesh(parent, visual, geometry) {
const meshEl = directChild(geometry, 'mesh');
const uri = getDirectText(meshEl, 'uri');
if (!uri) return;
const url = gazeboUriToPath(uri);
const scale = parseScale(getDirectText(meshEl, 'scale', '1 1 1'));
const material = materialFromVisual(visual);
if (/\.stl$/i.test(url)) {
await new Promise(resolve => {
stlLoader.load(url, geo => {
geo.computeVertexNormals();
const mesh = new THREE.Mesh(geo, material);
mesh.scale.copy(scale);
mesh.castShadow = true;
mesh.receiveShadow = true;
parent.add(mesh);
resolve();
}, undefined, err => {
console.warn('STL load failed:', url, err);
parent.add(new THREE.Mesh(new THREE.BoxGeometry(.1,.1,.1), new THREE.MeshLambertMaterial({ color:0xff5555 })));
resolve();
});
});
} else {
await new Promise(resolve => {
colladaLoader.load(url, collada => {
const obj = collada.scene;
obj.scale.multiply(scale);
prepMesh(obj, material);
parent.add(obj);
resolve();
}, undefined, err => {
console.warn('DAE load failed:', url, err);
parent.add(new THREE.Mesh(new THREE.BoxGeometry(.4,.4,.4), new THREE.MeshLambertMaterial({ color:0xff5555 })));
resolve();
});
});
}
}
function addBox(parent, visual, geometry) {
const box = directChild(geometry, 'box');
const s = parseNums(getDirectText(box, 'size', '1 1 1'), 3);
const mesh = new THREE.Mesh(new THREE.BoxGeometry(s[0], s[1], s[2]), materialFromVisual(visual));
mesh.castShadow = true; mesh.receiveShadow = true;
parent.add(mesh);
}
function addCylinder(parent, visual, geometry) {
const cyl = directChild(geometry, 'cylinder');
const radius = Number(getDirectText(cyl, 'radius', '0.1')) || .1;
const length = Number(getDirectText(cyl, 'length', '1')) || 1;
const mesh = new THREE.Mesh(new THREE.CylinderGeometry(radius, radius, length, 32), materialFromVisual(visual));
// Three cylinder 기본 축은 Y, SDF cylinder 기본 축은 Z입니다.
mesh.rotation.x = Math.PI / 2;
mesh.castShadow = true; mesh.receiveShadow = true;
parent.add(mesh);
}
function addSphere(parent, visual, geometry) {
const sph = directChild(geometry, 'sphere');
const radius = Number(getDirectText(sph, 'radius', '0.1')) || .1;
const mesh = new THREE.Mesh(new THREE.SphereGeometry(radius, 32, 16), materialFromVisual(visual));
mesh.castShadow = true; mesh.receiveShadow = true;
parent.add(mesh);
}
async function buildFromSdf() {
const xmlText = await fetch(MODEL_SDF).then(r => {
if (!r.ok) throw new Error(`${MODEL_SDF} load failed: ${r.status}`);
return r.text();
});
const doc = new DOMParser().parseFromString(xmlText, 'application/xml');
if (doc.querySelector('parsererror')) throw new Error(doc.querySelector('parsererror').textContent);
const model = doc.querySelector('model');
const modelGroup = new THREE.Group();
modelGroup.name = model?.getAttribute('name') || 'turtlebot3_world';
applySdfPose(modelGroup, parsePose(getDirectText(model, 'pose', '0 0 0 0 0 0')));
sdfRoot.add(modelGroup);
let loaded = 0;
for (const link of Array.from(model.querySelectorAll(':scope > link'))) {
const linkGroup = new THREE.Group();
linkGroup.name = link.getAttribute('name') || 'link';
applySdfPose(linkGroup, parsePose(getDirectText(link, 'pose', '0 0 0 0 0 0')));
modelGroup.add(linkGroup);
for (const visual of Array.from(link.querySelectorAll(':scope > visual'))) {
const visualGroup = new THREE.Group();
visualGroup.name = visual.getAttribute('name') || 'visual';
applySdfPose(visualGroup, parsePose(getDirectText(visual, 'pose', '0 0 0 0 0 0')));
linkGroup.add(visualGroup);
const geometry = directChild(visual, 'geometry');
if (!geometry) continue;
if (directChild(geometry, 'mesh')) await addMesh(visualGroup, visual, geometry);
else if (directChild(geometry, 'box')) addBox(visualGroup, visual, geometry);
else if (directChild(geometry, 'cylinder')) addCylinder(visualGroup, visual, geometry);
else if (directChild(geometry, 'sphere')) addSphere(visualGroup, visual, geometry);
loaded++;
}
}
if (!loaded) console.warn(`no visuals loaded from ${MODEL_SDF}`);
}
async function loadBurgerModel(sdfPose = { x:0, y:0, z:0, roll:0, pitch:0, yaw:0 }) {
const BURGER_SDF = './turtlebot3_world_model/turtlebot3_burger/model.sdf';
const xmlText = await fetch(BURGER_SDF).then(r => {
if (!r.ok) throw new Error(`burger SDF load failed: ${r.status}`);
return r.text();
});
const doc = new DOMParser().parseFromString(xmlText, 'application/xml');
const model = doc.querySelector('model');
const robotGroup = new THREE.Group();
robotGroup.name = 'turtlebot3_burger';
applySdfPose(robotGroup, sdfPose);
for (const link of Array.from(model.querySelectorAll(':scope > link'))) {
const linkGroup = new THREE.Group();
linkGroup.name = link.getAttribute('name') || 'link';
applySdfPose(linkGroup, parsePose(getDirectText(link, 'pose', '0 0 0 0 0 0')));
robotGroup.add(linkGroup);
for (const visual of Array.from(link.querySelectorAll(':scope > visual'))) {
const vg = new THREE.Group();
vg.name = visual.getAttribute('name') || 'visual';
applySdfPose(vg, parsePose(getDirectText(visual, 'pose', '0 0 0 0 0 0')));
linkGroup.add(vg);
const geometry = directChild(visual, 'geometry');
if (!geometry) continue;
if (directChild(geometry, 'mesh')) await addMesh(vg, visual, geometry);
else if (directChild(geometry, 'cylinder')) addCylinder(vg, visual, geometry);
}
}
sdfRoot.add(robotGroup);
robotModel = robotGroup;
}
const LASER_HOLE_R = 0.14;
const LASER_OFFSET_X = -0.032;
const LASER_OFFSET_Y = 0;
const LASER_Z = 0.181;
function setRobotPose(pose) {
if (!pose) return;
targetRobotPose = {
x: Number(pose.x ?? targetRobotPose.x),
y: Number(pose.y ?? targetRobotPose.y),
z: Number(pose.z ?? targetRobotPose.z),
roll: Number(pose.roll ?? targetRobotPose.roll),
pitch: Number(pose.pitch ?? targetRobotPose.pitch),
yaw: Number(pose.yaw ?? targetRobotPose.yaw),
};
if (!hasRobotPose) {
currentRobotPose = { ...targetRobotPose };
applyCurrentRobotPose();
hasRobotPose = true;
}
}
function lerpAngle(from, to, alpha) {
let delta = ((to - from + Math.PI) % (Math.PI * 2)) - Math.PI;
if (delta < -Math.PI) delta += Math.PI * 2;
return from + delta * alpha;
}
function applyCurrentRobotPose() {
if (!robotModel) return;
robotModel.position.set(currentRobotPose.x, currentRobotPose.y, currentRobotPose.z);
robotModel.rotation.set(currentRobotPose.roll, currentRobotPose.pitch, currentRobotPose.yaw, 'XYZ');
}
function updateSmoothedRobotPose() {
if (!hasRobotPose) return;
const alpha = 0.22;
currentRobotPose.x += (targetRobotPose.x - currentRobotPose.x) * alpha;
currentRobotPose.y += (targetRobotPose.y - currentRobotPose.y) * alpha;
currentRobotPose.z += (targetRobotPose.z - currentRobotPose.z) * alpha;
currentRobotPose.roll = lerpAngle(currentRobotPose.roll, targetRobotPose.roll, alpha);
currentRobotPose.pitch = lerpAngle(currentRobotPose.pitch, targetRobotPose.pitch, alpha);
currentRobotPose.yaw = lerpAngle(currentRobotPose.yaw, targetRobotPose.yaw, alpha);
applyCurrentRobotPose();
updateRobotPath();
updateFollowCamera();
}
function disposeObject(obj) {
obj.traverse(child => {
child.geometry?.dispose?.();
if (Array.isArray(child.material)) child.material.forEach(m => m.dispose?.());
else child.material?.dispose?.();
});
}
function updateRobotPath() {
const p = new THREE.Vector3(currentRobotPose.x, currentRobotPose.y, currentRobotPose.z + 0.03);
const last = robotPathPoints[robotPathPoints.length - 1];
if (!last || last.distanceTo(p) > 0.03) {
robotPathPoints.push(p);
if (robotPathPoints.length > 3000) robotPathPoints.shift();
}
if (!pathVisible) return;
if (!robotPathLine) {
robotPathLine = new THREE.Line(
new THREE.BufferGeometry(),
new THREE.LineBasicMaterial({ color: 0xffcc00, linewidth: 2 })
);
sdfRoot.add(robotPathLine);
}
robotPathLine.geometry.dispose();
robotPathLine.geometry = new THREE.BufferGeometry().setFromPoints(robotPathPoints);
}
function setPathVisible(visible) {
pathVisible = visible;
pathToggleButton.classList.toggle('active', visible);
if (!visible && robotPathLine) {
sdfRoot.remove(robotPathLine);
disposeObject(robotPathLine);
robotPathLine = null;
} else if (visible) {
updateRobotPath();
}
}
function laserOriginFromPose(pose) {
const q = new THREE.Quaternion().setFromEuler(new THREE.Euler(pose.roll || 0, pose.pitch || 0, pose.yaw || 0, 'XYZ'));
const offset = new THREE.Vector3(LASER_OFFSET_X, LASER_OFFSET_Y, LASER_Z).applyQuaternion(q);
return new THREE.Vector3(pose.x, pose.y, pose.z || 0).add(offset);
}
function laserDirectionFromPose(pose, angle) {
const q = new THREE.Quaternion().setFromEuler(new THREE.Euler(pose.roll || 0, pose.pitch || 0, pose.yaw || 0, 'XYZ'));
return new THREE.Vector3(Math.cos(angle), Math.sin(angle), 0).applyQuaternion(q).normalize();
}
function drawLaserScan(scan) {
if (!scan?.ranges?.length) return;
if (laserGroup) {
sdfRoot.remove(laserGroup);
disposeObject(laserGroup);
}
const origin = laserOriginFromPose(currentRobotPose);
const rangeMax = Number(scan.range_max || 3.5);
const rangeMin = Number(scan.range_min || 0.12);
const angleMin = Number(scan.angle_min || 0);
const angleInc = Number(scan.angle_increment || (Math.PI * 2 / scan.ranges.length));
const endpoints = [];
const linePoints = [];
for (let i = 0; i < scan.ranges.length; i++) {
const raw = scan.ranges[i];
const valid = Number.isFinite(raw) && raw >= rangeMin && raw < rangeMax;
const d = valid ? Math.min(raw, rangeMax) : rangeMax;
const a = angleMin + i * angleInc;
const dir = laserDirectionFromPose(currentRobotPose, a);
const startD = Math.min(LASER_HOLE_R, d);
const start = origin.clone().addScaledVector(dir, startD);
const point = origin.clone().addScaledVector(dir, d);
endpoints.push({ start, point, hit: valid });
linePoints.push(start.x, start.y, start.z, point.x, point.y, point.z);
}
const fillPoints = [];
const openFillPoints = [];
for (let i = 0; i < endpoints.length; i++) {
const curr = endpoints[i];
const next = endpoints[(i + 1) % endpoints.length];
const targetPoints = curr.hit && next.hit ? fillPoints : openFillPoints;
targetPoints.push(curr.start.x, curr.start.y, curr.start.z);
targetPoints.push(curr.point.x, curr.point.y, curr.point.z);
targetPoints.push(next.point.x, next.point.y, next.point.z);
targetPoints.push(curr.start.x, curr.start.y, curr.start.z);
targetPoints.push(next.point.x, next.point.y, next.point.z);
targetPoints.push(next.start.x, next.start.y, next.start.z);
}
const grp = new THREE.Group();
const openFillMat = new THREE.MeshBasicMaterial({
color: 0x3390ff,
transparent: true,
opacity: 0.18,
side: THREE.DoubleSide,
depthWrite: false,
});
const fillMat = new THREE.MeshBasicMaterial({
color: 0x005cff,
transparent: true,
opacity: 0.6,
side: THREE.DoubleSide,
depthWrite: false,
});
const lineMat = new THREE.LineBasicMaterial({ color: 0x3366ff, transparent: true, opacity: 0.5 });
if (openFillPoints.length) {
const geo = new THREE.BufferGeometry();
geo.setAttribute('position', new THREE.Float32BufferAttribute(openFillPoints, 3));
grp.add(new THREE.Mesh(geo, openFillMat));
}
if (fillPoints.length) {
const geo = new THREE.BufferGeometry();
geo.setAttribute('position', new THREE.Float32BufferAttribute(fillPoints, 3));
grp.add(new THREE.Mesh(geo, fillMat));
}
const lineGeo = new THREE.BufferGeometry();
lineGeo.setAttribute('position', new THREE.Float32BufferAttribute(linePoints, 3));
grp.add(new THREE.LineSegments(lineGeo, lineMat));
const frontIndex = Math.max(0, Math.min(scan.ranges.length - 1, Math.round((0 - angleMin) / angleInc)));
const front = endpoints[frontIndex];
if (front) {
const delta = new THREE.Vector3(
front.point.x - front.start.x,
front.point.y - front.start.y,
front.point.z - front.start.z
);
const len = delta.length();
if (len > 0.01) {
const beam = new THREE.Mesh(
new THREE.CylinderGeometry(0.006, 0.006, len, 16),
new THREE.MeshBasicMaterial({ color: 0x005cff })
);
beam.position.set(
(front.start.x + front.point.x) / 2,
(front.start.y + front.point.y) / 2,
(front.start.z + front.point.z) / 2
);
beam.quaternion.setFromUnitVectors(new THREE.Vector3(0, 1, 0), delta.normalize());
grp.add(beam);
}
}
laserGroup = grp;
sdfRoot.add(grp);
}
function drawMiniMap(state) {
if (!miniCtx) return;
const w = miniMap.width;
const h = miniMap.height;
miniCtx.clearRect(0, 0, w, h);
miniCtx.fillStyle = '#60736f';
miniCtx.fillRect(0, 0, w, h);
if (latestMiniMapGrid?.data?.length && latestMiniMapGrid.width && latestMiniMapGrid.height && !miniMapBitmap) {
const grid = latestMiniMapGrid;
const img = miniCtx.createImageData(grid.width, grid.height);
for (let y = 0; y < grid.height; y++) {
for (let x = 0; x < grid.width; x++) {
const src = (grid.height - 1 - y) * grid.width + x;
const dst = (y * grid.width + x) * 4;
const v = grid.data[src];
let c = 160;
if (v < 0) c = 112;
else if (v === 0) c = 214;
else c = Math.max(32, 210 - Math.round(v * 1.8));
img.data[dst] = c;
img.data[dst + 1] = c;
img.data[dst + 2] = c;
img.data[dst + 3] = 255;
}
}
miniMapBitmap = document.createElement('canvas');
miniMapBitmap.width = grid.width;
miniMapBitmap.height = grid.height;
miniMapBitmap.getContext('2d').putImageData(img, 0, 0);
}
const grid = latestMiniMapGrid;
if (grid && miniMapBitmap) {
const scale = Math.min(w / grid.width, h / grid.height);
const dw = grid.width * scale;
const dh = grid.height * scale;
const dx = (w - dw) / 2;
const dy = (h - dh) / 2;
miniCtx.imageSmoothingEnabled = false;
miniCtx.drawImage(miniMapBitmap, dx, dy, dw, dh);
const worldToMini = (wx, wy) => ({
x: dx + ((wx - grid.origin.x) / grid.resolution) * scale,
y: dy + (grid.height - (wy - grid.origin.y) / grid.resolution) * scale,
});
const mapFrame = { x:0, y:0, yaw:0 };
const odomFrame = state?.tf?.map_to_odom || { x:0, y:0, yaw:0 };
const robotFrame = state?.pose_map || currentRobotPose;
const frameLen = Math.max(grid.resolution * 12, 0.18);
if (state?.scan?.ranges?.length && (state?.pose_map || state?.pose)) {
const c = Math.cos(robotFrame.yaw);
const s = Math.sin(robotFrame.yaw);
const laserOrigin = {
x: robotFrame.x + c * LASER_OFFSET_X - s * LASER_OFFSET_Y,
y: robotFrame.y + s * LASER_OFFSET_X + c * LASER_OFFSET_Y,
};
const angleMin = Number(state.scan.angle_min || 0);
const angleInc = Number(state.scan.angle_increment || (Math.PI * 2 / state.scan.ranges.length));
const rangeMin = Number(state.scan.range_min || 0.12);
const rangeMax = Number(state.scan.range_max || 3.5);
const blink = 0.45 + 0.45 * Math.abs(Math.sin(Date.now() / 180));
miniCtx.fillStyle = `rgba(255, 0, 0, ${blink.toFixed(3)})`;
for (let i = 0; i < state.scan.ranges.length; i += 2) {
const d = state.scan.ranges[i];
if (!Number.isFinite(d) || d < rangeMin || d >= rangeMax) continue;
const a = robotFrame.yaw + angleMin + i * angleInc;
const p = worldToMini(
laserOrigin.x + Math.cos(a) * d,
laserOrigin.y + Math.sin(a) * d
);
miniCtx.beginPath();
miniCtx.arc(p.x, p.y, 1.2, 0, Math.PI * 2);
miniCtx.fill();
}
}
function drawLink(a, b) {
const p0 = worldToMini(a.x, a.y);
const p1 = worldToMini(b.x, b.y);
miniCtx.strokeStyle = 'rgba(255, 230, 0, .9)';
miniCtx.lineWidth = 1;
miniCtx.beginPath();
miniCtx.moveTo(p0.x, p0.y);
miniCtx.lineTo(p1.x, p1.y);
miniCtx.stroke();
}
function drawFrame(frame, label) {
const o = worldToMini(frame.x, frame.y);
const xEnd = worldToMini(frame.x + Math.cos(frame.yaw) * frameLen, frame.y + Math.sin(frame.yaw) * frameLen);
const yEnd = worldToMini(frame.x - Math.sin(frame.yaw) * frameLen, frame.y + Math.cos(frame.yaw) * frameLen);
miniCtx.lineWidth = 2;
miniCtx.strokeStyle = '#d71920';
miniCtx.beginPath();
miniCtx.moveTo(o.x, o.y);
miniCtx.lineTo(xEnd.x, xEnd.y);
miniCtx.stroke();
miniCtx.strokeStyle = '#22aa33';
miniCtx.beginPath();
miniCtx.moveTo(o.x, o.y);
miniCtx.lineTo(yEnd.x, yEnd.y);
miniCtx.stroke();
miniCtx.fillStyle = '#005cff';
miniCtx.beginPath();
miniCtx.arc(o.x, o.y, 3, 0, Math.PI * 2);
miniCtx.fill();
miniCtx.fillStyle = 'rgba(255,255,255,.92)';
miniCtx.strokeStyle = 'rgba(0,0,0,.75)';
miniCtx.lineWidth = 3;
miniCtx.font = '11px sans-serif';
miniCtx.strokeText(label, o.x + 5, o.y - 5);
miniCtx.fillText(label, o.x + 5, o.y - 5);
}
drawLink(mapFrame, odomFrame);
if (state?.pose || state?.pose_map) drawLink(odomFrame, robotFrame);
drawFrame(mapFrame, 'map');
drawFrame(odomFrame, 'odom');
if (state?.pose || state?.pose_map) drawFrame(robotFrame, 'robot');
return;
}
miniCtx.fillStyle = 'rgba(255,255,255,.85)';
miniCtx.font = '12px sans-serif';
miniCtx.fillText('waiting for /map', 14, 24);
}
function updateMiniMapMetadata(map) {
if (!map?.width || !map?.height) return;
latestMiniMapGrid = {
...(latestMiniMapGrid || {}),
...map,
data: latestMiniMapGrid?.data || null,
};
}
function applyMapBinary(buffer) {
const view = new DataView(buffer);
if (
buffer.byteLength < 12 ||
view.getUint8(0) !== 0x4d ||
view.getUint8(1) !== 0x41 ||
view.getUint8(2) !== 0x50 ||
view.getUint8(3) !== 0x31
) {
console.warn('unknown binary websocket frame');
return;
}
const width = view.getUint32(4, true);
const height = view.getUint32(8, true);
const data = new Int8Array(buffer, 12);
if (data.length !== width * height) {
console.warn(`invalid map binary size: ${data.length} != ${width * height}`);
return;
}
latestMiniMapGrid = {
...(latestMiniMapGrid || {}),
width,
height,
data,
};
miniMapBitmap = null;
drawMiniMap(latestSlamState);
downloadMapButton.disabled = false;
}
function applyScanBinary(buffer) {
const view = new DataView(buffer);
if (
buffer.byteLength < 8 ||
view.getUint8(0) !== 0x53 ||
view.getUint8(1) !== 0x43 ||
view.getUint8(2) !== 0x4e ||
view.getUint8(3) !== 0x31
) {
console.warn('unknown binary websocket frame');
return;
}
const count = view.getUint32(4, true);
if (buffer.byteLength !== 8 + count * 4) {
console.warn(`invalid scan binary size: ${buffer.byteLength} != ${8 + count * 4}`);
return;
}
latestScanRanges = new Float32Array(buffer, 8, count);
if (!latestScanMetadata) return;
const scan = { ...latestScanMetadata, ranges: latestScanRanges };
drawLaserScan(scan);
drawMiniMap({ ...(latestSlamState || {}), scan });
}
function applyBinaryFrame(buffer) {
if (buffer.byteLength < 4) return;
const view = new DataView(buffer, 0, 4);
const magic = String.fromCharCode(
view.getUint8(0),
view.getUint8(1),
view.getUint8(2),
view.getUint8(3)
);
if (magic === 'MAP1') applyMapBinary(buffer);
else if (magic === 'SCN1') applyScanBinary(buffer);
else console.warn(`unknown binary websocket frame: ${magic}`);
}
async function applySocketBinaryPayload(payload) {
if (payload instanceof ArrayBuffer) {
applyBinaryFrame(payload);
return true;
}
if (payload instanceof Blob) {
applyBinaryFrame(await payload.arrayBuffer());
return true;
}
if (ArrayBuffer.isView(payload)) {
applyBinaryFrame(payload.buffer.slice(payload.byteOffset, payload.byteOffset + payload.byteLength));
return true;
}
return false;
}
function mapToPcd(grid) {
const points = [];
const resolution = Number(grid.resolution || 0);
const originX = Number(grid.origin?.x || 0);
const originY = Number(grid.origin?.y || 0);
const originYaw = Number(grid.origin?.yaw || 0);
const c = Math.cos(originYaw);
const s = Math.sin(originYaw);
for (let y = 0; y < grid.height; y++) {
for (let x = 0; x < grid.width; x++) {
const value = grid.data[y * grid.width + x];
if (value < 50) continue;
const lx = (x + 0.5) * resolution;
const ly = (y + 0.5) * resolution;
const wx = originX + c * lx - s * ly;
const wy = originY + s * lx + c * ly;
points.push(`${wx.toFixed(4)} ${wy.toFixed(4)} 0.0000`);
}
}
return [
'# .PCD v0.7 - Point Cloud Data file format',
'VERSION 0.7',
'FIELDS x y z',
'SIZE 4 4 4',
'TYPE F F F',
'COUNT 1 1 1',
`WIDTH ${points.length}`,
'HEIGHT 1',
'VIEWPOINT 0 0 0 1 0 0 0',
`POINTS ${points.length}`,
'DATA ascii',
...points,
'',
].join('\n');
}
function mapToPgmBytes(grid) {
const header = `P5\n# CREATOR: slam-test web viewer\n${grid.width} ${grid.height}\n255\n`;
const headerBytes = new TextEncoder().encode(header);
const pixels = new Uint8Array(grid.width * grid.height);
for (let y = 0; y < grid.height; y++) {
for (let x = 0; x < grid.width; x++) {
const src = (grid.height - 1 - y) * grid.width + x;
const dst = y * grid.width + x;
const v = grid.data[src];
if (v < 0) pixels[dst] = 205;
else if (v === 0) pixels[dst] = 254;
else pixels[dst] = Math.max(0, 254 - Math.round(v * 2.54));
}
}
const out = new Uint8Array(headerBytes.length + pixels.length);
out.set(headerBytes, 0);
out.set(pixels, headerBytes.length);
return out;
}
function mapToYaml(grid, imageName) {
return [
`image: ${imageName}`,
'mode: trinary',
`resolution: ${Number(grid.resolution || 0)}`,
`origin: [${Number(grid.origin?.x || 0)}, ${Number(grid.origin?.y || 0)}, ${Number(grid.origin?.yaw || 0)}]`,
'negate: 0',
'occupied_thresh: 0.65',
'free_thresh: 0.25',
'',
].join('\n');
}
function downloadBlob(blob, filename) {
const url = URL.createObjectURL(blob);
const a = document.createElement('a');
a.href = url;
a.download = filename;
document.body.appendChild(a);
a.click();
a.remove();
URL.revokeObjectURL(url);
}
function downloadMap(format) {
if (!latestMiniMapGrid?.data?.length) return;
const stamp = new Date().toISOString().replace(/[:.]/g, '-');
const baseName = `slam-map-${stamp}`;
if (format === 'pcd') {
downloadBlob(new Blob([mapToPcd(latestMiniMapGrid)], { type:'application/octet-stream' }), `${baseName}.pcd`);
} else if (format === 'pgm') {
downloadBlob(new Blob([mapToPgmBytes(latestMiniMapGrid)], { type:'image/x-portable-graymap' }), `${baseName}.pgm`);
} else if (format === 'yaml') {
downloadBlob(new Blob([mapToYaml(latestMiniMapGrid, `${baseName}.pgm`)], { type:'text/yaml' }), `${baseName}.yaml`);
}
}
downloadMapButton.addEventListener('click', () => {
if (downloadMapButton.disabled) return;
downloadMenu.classList.toggle('open');
focusViewer();
});
downloadMenu.addEventListener('click', (event) => {
const format = event.target?.dataset?.format;
if (!format) return;
downloadMenu.classList.remove('open');
downloadMap(format);
focusViewer();
});
window.addEventListener('pointerdown', (event) => {
if (!event.target.closest?.('#downloadWrap')) {
downloadMenu.classList.remove('open');
}
});
pathToggleButton.addEventListener('click', () => {
setPathVisible(!pathVisible);
focusViewer();
});
goalToggleButton.addEventListener('click', () => {
navigationGoalMode = !navigationGoalMode;
goalToggleButton.classList.toggle('active', navigationGoalMode);
focusViewer();
});
miniMapToggle.addEventListener('click', () => {
const hidden = miniMap.classList.toggle('hidden');
miniMapToggle.textContent = hidden ? '▴' : '▾';
miniMapToggle.setAttribute('aria-label', hidden ? '미니맵 펼치기' : '미니맵 접기');
focusViewer();
});
const DEFAULT_CAMERA = {
position: new THREE.Vector3(4, 6, 5),
target: new THREE.Vector3(0, 0, 0),
};
function sdfPointToWorld(x, y, z = 0) {
const point = new THREE.Vector3(x, y, z);
sdfRoot.updateMatrixWorld();
return sdfRoot.localToWorld(point);
}
function worldPointToSdf(point) {
sdfRoot.updateMatrixWorld();
return sdfRoot.worldToLocal(point.clone());
}
function placeNavigationGoalMarker(x, y, yaw = 0) {
if (navigationGoalMarker) {
sdfRoot.remove(navigationGoalMarker);
disposeObject(navigationGoalMarker);
}
const marker = new THREE.Group();
marker.position.set(x, y, 0.05);
marker.rotation.z = yaw;
const ring = new THREE.Mesh(
new THREE.TorusGeometry(0.26, 0.025, 8, 36),
new THREE.MeshBasicMaterial({ color:0x22c55e })
);
marker.add(ring);
sdfRoot.add(marker);
navigationGoalMarker = marker;
}
function pickNavigationGoal(event) {
const rect = renderer.domElement.getBoundingClientRect();
const mouse = new THREE.Vector2(
((event.clientX - rect.left) / rect.width) * 2 - 1,
-((event.clientY - rect.top) / rect.height) * 2 + 1
);
navigationRaycaster.setFromCamera(mouse, camera);
const worldPoint = new THREE.Vector3();
if (!navigationRaycaster.ray.intersectPlane(navigationGroundPlane, worldPoint)) return null;
const sdfPoint = worldPointToSdf(worldPoint);
return {
x: sdfPoint.x,
y: sdfPoint.y,
yaw: currentRobotPose.yaw || 0,
};
}
async function sendNavigationGoal(goal) {
placeNavigationGoalMarker(goal.x, goal.y, goal.yaw);
const response = await fetch(`${BACKEND_URL}/navigate_to_pose`, {
method:'POST',
headers:{ 'Content-Type':'application/json' },
body:JSON.stringify({ ...goal, frame_id:'map' }),
});
if (!response.ok) {
const message = await response.text().catch(() => '');
throw new Error(message || 'navigation goal failed');
}
return response.json().catch(() => ({ ok:true }));
}
function applyCameraPreset(view) {
activeCameraPreset = view || 'default';
if (view === 'top') {
const target = sdfPointToWorld(currentRobotPose.x, currentRobotPose.y, 0);
camera.position.copy(target).add(new THREE.Vector3(0, 12, 0));
camera.up.set(1, 0, 0);
controls.target.copy(target);
} else if (view === 'follow') {
updateFollowCamera(true);
return;
} else {
camera.position.copy(DEFAULT_CAMERA.position);
camera.up.set(0, 1, 0);
controls.target.copy(DEFAULT_CAMERA.target);
}
camera.lookAt(controls.target);
controls.update();
}
function updateFollowCamera(force = false) {
if (activeCameraPreset !== 'follow' && !force) return;
const desiredTarget = sdfPointToWorld(currentRobotPose.x, currentRobotPose.y, currentRobotPose.z);
const desiredPosition = desiredTarget.clone().add(new THREE.Vector3(0, 4.0, 0));
camera.up.set(1, 0, 0);
if (force) {
camera.position.copy(desiredPosition);
controls.target.copy(desiredTarget);
} else {
camera.position.lerp(desiredPosition, 0.25);
controls.target.lerp(desiredTarget, 0.25);
}
camera.lookAt(controls.target);
controls.update();
}
cameraPresetSelect.addEventListener('change', () => {
applyCameraPreset(cameraPresetSelect.value);
focusViewer();
});
renderer.domElement.addEventListener('click', async (event) => {
if (!navigationGoalMode) return;
event.preventDefault();
event.stopPropagation();
const goal = pickNavigationGoal(event);
if (!goal) return;
try {
await sendNavigationGoal(goal);
navigationGoalMode = false;
goalToggleButton.classList.remove('active');
} catch (error) {
console.warn('navigation goal failed:', error);
}
});
function applySlamState(state) {
latestSlamState = state;
if (state?.pose) setRobotPose(state.pose);
if (state?.scan) {
latestScanMetadata = state.scan;
if (latestScanRanges) {
const scan = { ...latestScanMetadata, ranges: latestScanRanges };
drawLaserScan(scan);
state = { ...state, scan };
latestSlamState = state;
}
}
if (state?.map) updateMiniMapMetadata(state.map);
drawMiniMap(state);
downloadMapButton.disabled = !latestMiniMapGrid?.data?.length;
}
function connectSlamSocket() {
slamSocket = new WebSocket(BACKEND_WS);
slamSocket.binaryType = 'arraybuffer';
slamSocket.addEventListener('message', async (event) => {
if (await applySocketBinaryPayload(event.data)) {
return;
}
try {
applySlamState(JSON.parse(event.data));
} catch (err) {
console.warn('invalid slam websocket payload:', err);
}
});
slamSocket.addEventListener('close', () => {
console.warn(`SLAM websocket disconnected. reconnecting... ${BACKEND_WS}`);
setTimeout(connectSlamSocket, 1000);
});
slamSocket.addEventListener('error', () => {
slamSocket.close();
});
}
let maxLinear = 0.22;
let maxAngular = 2.84;
let activeMoveKey = null;
const moveBindings = {
u: [ 1, 1], i: [ 1, 0], o: [ 1, -1],
j: [ 0, 1], k: [ 0, 0], l: [ 0, -1],
m: [-1, -1], ',': [-1, 0], '.': [-1, 1],
};
const speedBindings = {
q: [1.1, 1.1], z: [0.9, 0.9],
w: [1.1, 1.0], x: [0.9, 1.0],
e: [1.0, 1.1], c: [1.0, 0.9],
};
function canUseTeleop(event) {
if (!document.hasFocus()) return false;
const tag = event.target?.tagName?.toLowerCase();
return !['input', 'textarea', 'select', 'button'].includes(tag);
}
function sendCmdVel(linear, angular) {
const payload = { type:'cmd_vel', linear, angular };
if (!DIRECT_BRIDGE && slamSocket?.readyState === WebSocket.OPEN) {
slamSocket.send(JSON.stringify(payload));
return;
}
fetch(`${BACKEND_URL}/cmd_vel`, {
method:'POST',
headers:{ 'Content-Type':'application/json' },
body:JSON.stringify({ linear, angular }),
}).catch(console.warn);
}
function stopRobot() {
sendCmdVel(0, 0);
}
window.addEventListener('keydown', (event) => {
if (!canUseTeleop(event)) return;
const key = event.key.toLowerCase();
if (speedBindings[key]) {
event.preventDefault();
const [linearScale, angularScale] = speedBindings[key];
maxLinear = Math.max(0.01, maxLinear * linearScale);
maxAngular = Math.max(0.1, maxAngular * angularScale);
return;
}
if (!moveBindings[key]) return;
event.preventDefault();
activeMoveKey = key;
const [linearSign, angularSign] = moveBindings[key];
sendCmdVel(linearSign * maxLinear, angularSign * maxAngular);
});
window.addEventListener('keyup', (event) => {
if (!canUseTeleop(event)) return;
const key = event.key.toLowerCase();
if (!moveBindings[key] || activeMoveKey !== key) return;
event.preventDefault();
activeMoveKey = null;
stopRobot();
});
window.addEventListener('blur', () => {
if (!activeMoveKey) return;
activeMoveKey = null;
stopRobot();
});
Promise.all([
buildFromSdf(),
loadBurgerModel({ x:-2.0, y:-0.5, z:0.01, roll:0, pitch:0, yaw:0 }),
]).then(() => {
connectSlamSocket();
})
.catch(err => {
console.error(err);
});
window.addEventListener('resize', () => {
camera.aspect = innerWidth / innerHeight;
camera.updateProjectionMatrix();
renderer.setSize(innerWidth, innerHeight);
});
function animate(){
requestAnimationFrame(animate);
updateSmoothedRobotPose();
controls.update();
renderer.render(scene, camera);
}
animate();
</script>
</body>
</html>