Files
2026-08-03 14:06:06 +09:00

2366 lines
81 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:#111520;
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;
}
#topLeftControls {
position:absolute;
left:16px;
top:16px;
z-index:10;
display:none;
}
#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);
}
#responseWaitOverlay {
position:fixed;
inset:0;
z-index:30;
display:flex;
align-items:center;
justify-content:center;
background:rgba(5,8,16,.58);
backdrop-filter:blur(2px);
transition:opacity .2s ease;
}
#responseWaitOverlay.is-hidden {
opacity:0;
pointer-events:none;
}
.response-wait-dialog {
display:flex;
align-items:center;
gap:10px;
padding:14px 18px;
border:1px solid rgba(255,255,255,.16);
border-radius:8px;
background:rgba(17,21,32,.94);
color:rgba(255,255,255,.9);
font:13px sans-serif;
font-weight:700;
box-shadow:0 18px 48px rgba(0,0,0,.3);
}
.response-wait-spinner {
width:16px;
height:16px;
border:2px solid rgba(255,255,255,.22);
border-top-color:#3b7ffa;
border-radius:50%;
animation:response-wait-spin .75s linear infinite;
}
@keyframes response-wait-spin {
to { transform:rotate(360deg); }
}
#downloadWrap {
position:relative;
}
#downloadMap,
#pathToggle,
#poseToggle,
#goalToggle,
#obstacleToggle,
#initToggle,
#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,
#poseToggle,
#goalToggle,
#obstacleToggle,
#initToggle {
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,
#poseToggle:hover,
#goalToggle:hover,
#obstacleToggle: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;
}
#goalToggle:disabled {
opacity:.42;
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;
}
#poseToggle.active {
background:rgba(37,99,235,.62);
border-color:rgba(147,197,253,.55);
color:#fff;
}
#obstacleToggle.active {
background:rgba(217,119,6,.62);
border-color:rgba(253,230,138,.62);
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;
}
.editor-mode #topRightControls,
.editor-mode #miniMap,
.editor-mode #miniMapToggle,
.editor-mode #keyGuide {
display:none;
}
.nav2-slam #goalToggle, .nav2-slam #poseToggle { display:none; }
.nav2-navigation #downloadWrap { display:none; }
.nav2-navigation #topLeftControls { display:block; }
.real-runtime #topLeftControls { display:none; }
</style>
</head>
<body>
<div id="topLeftControls">
<button id="obstacleToggle" title="벽 배치" aria-label="벽 배치" aria-pressed="false">Wall</button>
</div>
<div id="topRightControls">
<div id="downloadWrap">
<button id="downloadMap" title="맵 저장" aria-label="맵 저장" disabled><span>SAVE</span></button>
</div>
<button id="pathToggle" title="로봇 경로 표시" aria-label="로봇 경로 표시">Path</button>
<button id="poseToggle" title="초기 위치 지정" aria-label="초기 위치 지정">Pose</button>
<button id="goalToggle" title="목표 위치 지정" aria-label="목표 위치 지정">Goal</button>
<button id="initToggle" title="주행 초기화" aria-label="주행 초기화">Init</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>
<div id="responseWaitOverlay" role="status" aria-live="polite">
<div class="response-wait-dialog">
<span class="response-wait-spinner" aria-hidden="true"></span>
<span>응답을 기다리는 중입니다.</span>
</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 { TransformControls } from 'three/addons/controls/TransformControls.js';
import { ColladaLoader } from 'three/addons/loaders/ColladaLoader.js';
import { GLTFLoader } from 'three/addons/loaders/GLTFLoader.js';
import { STLLoader } from 'three/addons/loaders/STLLoader.js';
const viewerMapKey = new URLSearchParams(window.location.search).get('mapKey');
const params = new URLSearchParams(location.search);
const REAL_RUNTIME = params.get('runtime') === 'real';
const CUSTOM_WORLD = !REAL_RUNTIME && viewerMapKey === 'nav2-custom';
const activeWorldModel = viewerMapKey === 'nav2-turtlebot3-house' ? 'turtlebot3_house' : 'turtlebot3_world';
const MODEL_ROOT = `./turtlebot3_world_model/${activeWorldModel}`;
const MODEL_SDF = `${MODEL_ROOT}/model.sdf`;
const miniMap = document.getElementById('miniMap');
const miniCtx = miniMap.getContext('2d');
const miniMapToggle = document.getElementById('miniMapToggle');
const responseWaitOverlay = document.getElementById('responseWaitOverlay');
const downloadMapButton = document.getElementById('downloadMap');
const pathToggleButton = document.getElementById('pathToggle');
const poseToggleButton = document.getElementById('poseToggle');
const goalToggleButton = document.getElementById('goalToggle');
const obstacleToggleButton = document.getElementById('obstacleToggle');
const initToggleButton = document.getElementById('initToggle');
const cameraPresetSelect = document.getElementById('cameraPresetSelect');
const EDITOR_MODE = params.get('editor') === '1';
const NAV2_VIEW_MODE = params.get('mode') === 'navigation' ? 'navigation' : 'slam';
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');
const CONTROL_WS = DIRECT_BRIDGE ? BACKEND_WS.replace(/\/ws\/state(?:\?.*)?$/, '/ws/control') : null;
if (EDITOR_MODE) document.body.classList.add('editor-mode');
if (!EDITOR_MODE) document.body.classList.add(`nav2-${NAV2_VIEW_MODE}`);
if (REAL_RUNTIME) document.body.classList.add('real-runtime');
if (EDITOR_MODE) responseWaitOverlay?.classList.add('is-hidden');
let visualizationResponseReceived = EDITOR_MODE;
function markVisualizationResponseReceived() {
if (visualizationResponseReceived) return;
visualizationResponseReceived = true;
responseWaitOverlay?.classList.add('is-hidden');
}
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(20, 20),
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(20, 20, 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);
const editorObjects = new THREE.Group();
sdfRoot.add(editorObjects);
const runtimeLayoutObjects = new THREE.Group();
sdfRoot.add(runtimeLayoutObjects);
const editorRaycaster = new THREE.Raycaster();
const editorTransform = EDITOR_MODE ? new TransformControls(camera, renderer.domElement) : null;
let editorLayout = { version: 1, objects: [] };
let editorSelectedId = null;
let editorSelectionBox = null;
let editorSelectionGlow = null;
function updateEditorSelection() {
if (!editorSelectionBox || !editorSelectionGlow || !editorTransform?.object) return;
editorSelectionBox.update();
const bounds = new THREE.Box3().setFromObject(editorTransform.object);
const size = bounds.getSize(new THREE.Vector3()).multiplyScalar(1.12);
editorSelectionGlow.position.copy(bounds.getCenter(new THREE.Vector3()));
editorSelectionGlow.scale.copy(size);
}
if (editorTransform) {
editorTransform.setSize(1.25);
editorTransform.setSpace('local');
editorTransform.showZ = false;
editorTransform.addEventListener('dragging-changed', (event) => {
controls.enabled = !event.value;
});
editorTransform.addEventListener('mouseUp', () => {
const mesh = editorTransform.object;
if (!mesh?.userData?.objectId) return;
const original = editorLayout.objects.find((item) => item.id === mesh.userData.objectId);
if (!original) return;
const next = {
position: [mesh.position.x, mesh.position.y, Math.max(0.05, original.size[2] * mesh.scale.z / 2)],
rotation: [0, 0, mesh.rotation.z],
size: [
Math.max(0.1, original.size[0] * mesh.scale.x),
Math.max(0.1, original.size[1] * mesh.scale.y),
Math.max(0.1, original.size[2] * mesh.scale.z),
],
};
window.parent.postMessage({ source:'autonomy-world-editor', type:'transform-object', id: original.id, patch: next }, '*');
});
editorTransform.addEventListener('change', updateEditorSelection);
// Three.js r181에서는 TransformControls의 시각 기즈모를 helper로 씬에 추가한다.
scene.add(editorTransform.getHelper());
}
function attachEditorTransform() {
if (!editorTransform) return;
const mesh = editorObjects.children.find((child) => child.userData?.objectId === editorSelectedId);
if (editorSelectionBox) {
scene.remove(editorSelectionBox);
editorSelectionBox.geometry?.dispose?.();
editorSelectionBox.material?.dispose?.();
editorSelectionBox = null;
}
if (editorSelectionGlow) {
scene.remove(editorSelectionGlow);
editorSelectionGlow.geometry?.dispose?.();
editorSelectionGlow.material?.dispose?.();
editorSelectionGlow = null;
}
if (!mesh) {
editorTransform.detach();
return;
}
editorTransform.attach(mesh);
editorSelectionBox = new THREE.BoxHelper(mesh, 0x28a7ff);
editorSelectionBox.material.transparent = true;
editorSelectionBox.material.opacity = 0.95;
editorSelectionBox.material.depthTest = false;
editorSelectionBox.renderOrder = 1000;
scene.add(editorSelectionBox);
editorSelectionGlow = new THREE.Mesh(
new THREE.BoxGeometry(1, 1, 1),
new THREE.MeshBasicMaterial({ color:0x1d9bf0, transparent:true, opacity:0.16, depthWrite:false, depthTest:false, side:THREE.BackSide })
);
editorSelectionGlow.renderOrder = 999;
scene.add(editorSelectionGlow);
updateEditorSelection();
}
function setEditorTransformMode(mode) {
if (!editorTransform) return;
editorTransform.setMode(mode === 'rotate' || mode === 'scale' ? mode : 'translate');
editorTransform.showX = mode !== 'rotate';
editorTransform.showY = mode !== 'rotate';
editorTransform.showZ = mode === 'rotate';
editorTransform.showXY = mode !== 'rotate';
editorTransform.showXZ = false;
editorTransform.showYZ = false;
if (mode === 'rotate') {
editorTransform.showX = false;
editorTransform.showY = false;
editorTransform.showZ = true;
}
}
if (EDITOR_MODE) setEditorTransformMode('translate');
function clearLayoutObjects(parent) {
while (parent.children.length) {
const child = parent.children.pop();
child.geometry?.dispose?.();
child.material?.dispose?.();
}
}
function createLayoutMesh(item, editor = false) {
const size = Array.isArray(item.size) ? item.size : [1, 1, 1];
const position = Array.isArray(item.position) ? item.position : [0, 0, 0.5];
const rotation = Array.isArray(item.rotation) ? item.rotation : [0, 0, 0];
const type = item.type === 'pillar' ? 'pillar' : item.type === 'wall' ? 'wall' : 'box';
const material = new THREE.MeshStandardMaterial({
color: editor
? (type === 'pillar' ? 0xfb923c : type === 'wall' ? 0x60a5fa : 0xa78bfa)
: (type === 'pillar' ? 0x8aa6c6 : type === 'wall' ? 0x74869a : 0x919aa6),
roughness: 0.68,
side: THREE.DoubleSide,
});
const geometry = type === 'pillar'
? new THREE.CylinderGeometry(Math.max(size[0], size[1]) / 2, Math.max(size[0], size[1]) / 2, size[2], 32)
: new THREE.BoxGeometry(size[0], size[1], size[2]);
const mesh = new THREE.Mesh(geometry, material);
mesh.position.set(position[0], position[1], position[2]);
mesh.rotation.set(rotation[0], rotation[1], rotation[2]);
if (type === 'pillar') mesh.rotateX(Math.PI / 2);
mesh.castShadow = true;
mesh.receiveShadow = true;
mesh.userData.objectId = item.id;
return mesh;
}
function renderEditorLayout(layout) {
if (!EDITOR_MODE) return;
editorLayout = layout && Array.isArray(layout.objects) ? layout : { version:1, objects:[] };
clearLayoutObjects(editorObjects);
for (const item of editorLayout.objects) {
editorObjects.add(createLayoutMesh(item, true));
}
attachEditorTransform();
}
let runtimeLayoutSignature = '';
async function refreshRuntimeLayout() {
if (EDITOR_MODE || REAL_RUNTIME || !CUSTOM_WORLD) return;
try {
const layout = await fetch(`${BACKEND_URL}/world-layout`).then(response => response.ok ? response.json() : { objects: [] });
const signature = JSON.stringify(layout?.objects || []);
if (signature === runtimeLayoutSignature) return;
runtimeLayoutSignature = signature;
clearLayoutObjects(runtimeLayoutObjects);
for (const item of layout?.objects || []) runtimeLayoutObjects.add(createLayoutMesh(item));
} catch (_) {
// The visualization remains available while the Map module is offline.
}
}
window.addEventListener('message', (event) => {
if (!EDITOR_MODE || event.data?.source !== 'autonomy-world-editor') return;
if (event.data.type === 'set-layout') {
editorSelectedId = typeof event.data.selectedId === 'string' ? event.data.selectedId : null;
renderEditorLayout(event.data.layout);
}
if (event.data.type === 'set-transform-mode') setEditorTransformMode(event.data.mode);
});
let robotModel = null;
let posePreviewModel = null;
let laserGroup = null;
let slamSocket = null;
let controlSocket = 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 navigationPlanLine = null;
let navigationPlanSignature = '';
let navigationPlanSuppressed = NAV2_VIEW_MODE === 'navigation';
const runtimeObstacleObjects = new THREE.Group();
runtimeObstacleObjects.name = 'runtime-obstacles';
sdfRoot.add(runtimeObstacleObjects);
let activeCameraPreset = 'default';
let navigationGoalMode = false;
let initialPoseMode = false;
let initialPoseDrag = null;
let initialPoseDirectionArrow = null;
let obstaclePlacementMode = false;
let navigationInitialPoseSet = NAV2_VIEW_MODE !== 'navigation';
let initialPoseRequestSequence = 0;
let navigationMapToOdom = null;
let navigationGoalMarker = null;
let navigationGoalId = null;
let navigationGoalPose = null;
let navigationGoalPreview = null;
let obstaclePreview = null;
let obstaclePreviewTemplate = null;
const navigationRaycaster = new THREE.Raycaster();
const navigationGroundPlane = new THREE.Plane(new THREE.Vector3(0, 1, 0), 0);
function syncGoalToggleAvailability() {
const disabled = NAV2_VIEW_MODE === 'navigation' && !navigationInitialPoseSet;
goalToggleButton.disabled = disabled;
if (disabled) {
navigationGoalMode = false;
goalToggleButton.classList.remove('active');
hideNavigationGoalPreview();
}
}
syncGoalToggleAvailability();
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_house: './turtlebot3_world_model/turtlebot3_house',
turtlebot3_common: './turtlebot3_world_model/turtlebot3_common',
mailbox: './turtlebot3_world_model/mailbox',
};
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);
// mailbox.dae는 Z_UP인데 SDF 루트도 Z-up -> Y-up 변환을 한다.
// ColladaLoader의 변환을 한 번 상쇄해 Gazebo와 같은 방향으로 둔다.
if (uri.startsWith('model://mailbox/')) obj.rotation.x += Math.PI / 2;
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;
async function addModelVisuals(sdfModel, parent) {
const nestedModelGroup = new THREE.Group();
let directGeometryCount = 0;
nestedModelGroup.name = sdfModel.getAttribute('name') || 'model';
applySdfPose(nestedModelGroup, parsePose(getDirectText(sdfModel, 'pose', '0 0 0 0 0 0')));
parent.add(nestedModelGroup);
for (const link of Array.from(sdfModel.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')));
nestedModelGroup.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')) {
const uri = getDirectText(directChild(geometry, 'mesh'), 'uri');
// 번들에 포함한 메시만 읽고, 재질은 단색으로 덮어쓴다.
// 나머지 Gazebo 모델은 아래의 단순 형상으로 표시한다.
if (/^model:\/\/(turtlebot3_world|turtlebot3_house|turtlebot3_common|turtlebot3_burger|mailbox)\//.test(uri)) {
await addMesh(visualGroup, visual, geometry);
loaded++;
directGeometryCount++;
}
} else if (directChild(geometry, 'box')) {
addBox(visualGroup, visual, geometry);
loaded++;
directGeometryCount++;
} else if (directChild(geometry, 'cylinder')) {
addCylinder(visualGroup, visual, geometry);
loaded++;
directGeometryCount++;
} else if (directChild(geometry, 'sphere')) {
addSphere(visualGroup, visual, geometry);
loaded++;
directGeometryCount++;
}
}
}
for (const childModel of Array.from(sdfModel.querySelectorAll(':scope > model'))) {
await addModelVisuals(childModel, nestedModelGroup);
}
// House 월드의 mailbox, trash can, marble table 등은 Gazebo 기본
// 메시 파일만 참조합니다. 시각화에는 무거운 메시/텍스처 대신 위치와
// 점유 영역을 나타내는 간단한 형상을 둡니다.
const modelName = (sdfModel.getAttribute('name') || '').toLowerCase();
if (sdfModel !== model && directGeometryCount === 0 && modelName) {
const material = new THREE.MeshLambertMaterial({ color: modelName.includes('trash') ? 0x64748b : 0x9a7b5d });
let proxy;
if (modelName.includes('trash')) {
proxy = new THREE.Mesh(new THREE.CylinderGeometry(0.24, 0.2, 0.72, 20), material);
proxy.rotation.x = Math.PI / 2;
} else if (modelName.includes('mailbox')) {
proxy = new THREE.Mesh(new THREE.BoxGeometry(0.45, 0.28, 0.36), material);
proxy.position.z = 0.18;
} else {
proxy = new THREE.Mesh(new THREE.BoxGeometry(1.45, 0.85, 0.74), material);
proxy.position.z = 0.37;
}
proxy.castShadow = true;
proxy.receiveShadow = true;
nestedModelGroup.add(proxy);
loaded++;
}
}
// House 월드는 벽 외에도 책장, 캐비닛, 테이블을 하위 model로 정의합니다.
// 모델 계층을 함께 읽어 Gazebo와 동일한 장애물 배치를 단색 형상으로 그립니다.
await addModelVisuals(model, modelGroup);
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;
robotModel.visible = navigationInitialPoseSet;
posePreviewModel = robotGroup.clone(true);
posePreviewModel.name = 'turtlebot3_burger_pose_preview';
posePreviewModel.traverse((child) => {
if (!child.isMesh) return;
if (Array.isArray(child.material)) {
child.material = child.material.map((material) => material.clone());
child.material.forEach((material) => {
material.transparent = true;
material.opacity = 0.38;
material.depthWrite = false;
});
return;
}
child.material = child.material.clone();
child.material.transparent = true;
child.material.opacity = 0.38;
child.material.depthWrite = false;
});
posePreviewModel.visible = false;
sdfRoot.add(posePreviewModel);
}
async function loadMkMiniModel(sdfPose = { x:0, y:0, z:0, roll:0, pitch:0, yaw:0 }) {
const gltf = await new GLTFLoader().loadAsync('./assets/mk-mini.glb');
const robotGroup = new THREE.Group();
robotGroup.name = 'mkmini';
applySdfPose(robotGroup, sdfPose);
const model = gltf.scene;
// The source asset is Z-up; convert it to the viewer's Y-up coordinate system.
model.rotation.x = Math.PI / 2;
model.updateMatrixWorld(true);
const initialBounds = new THREE.Box3().setFromObject(model);
const initialSize = initialBounds.getSize(new THREE.Vector3());
const largestDimension = Math.max(initialSize.x, initialSize.y, initialSize.z, 0.001);
model.scale.setScalar(0.62 / largestDimension);
model.updateMatrixWorld(true);
const normalizedBounds = new THREE.Box3().setFromObject(model);
const center = normalizedBounds.getCenter(new THREE.Vector3());
// This model lives under sdfRoot (Gazebo Z-up). Center it on base_link in
// the horizontal plane and place its base on SDF Z=0.
model.position.set(-center.x, -center.y, -normalizedBounds.min.z);
model.traverse((child) => {
if (!child.isMesh) return;
child.castShadow = true;
child.receiveShadow = true;
});
robotGroup.add(model);
sdfRoot.add(robotGroup);
robotModel = robotGroup;
robotModel.visible = navigationInitialPoseSet;
posePreviewModel = robotGroup.clone(true);
posePreviewModel.name = 'mkmini_pose_preview';
posePreviewModel.traverse((child) => {
if (!child.isMesh) return;
child.material = Array.isArray(child.material)
? child.material.map((material) => material.clone())
: child.material.clone();
const materials = Array.isArray(child.material) ? child.material : [child.material];
materials.forEach((material) => {
material.transparent = true;
material.opacity = 0.38;
material.depthWrite = false;
});
});
posePreviewModel.visible = false;
sdfRoot.add(posePreviewModel);
}
const LASER_HOLE_R = 0.14;
const LASER_OFFSET_X = REAL_RUNTIME ? 0.1 : -0.032;
const LASER_OFFSET_Y = 0;
// Calibrated to the MKMini GLB at the current display scale.
const LASER_Z = REAL_RUNTIME ? 0.5 : 0.181;
// The MKMini ROS frame reports os_lidar at Z=0.6 m while the bundled GLB
// places the visible LiDAR head at approximately Z=0.5 m. Keep ROS/map
// coordinates untouched and correct only the rendered scan origin.
const MKMINI_LASER_RENDER_Z_OFFSET = 0.1;
function setRobotPose(pose) {
if (!pose) return;
if (NAV2_VIEW_MODE === 'navigation' && !navigationInitialPoseSet) 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;
} else if (activeMoveKey) {
// While teleoperating, render the latest ROS odometry immediately instead
// of letting interpolation accumulate behind the incoming state stream.
currentRobotPose = { ...targetRobotPose };
applyCurrentRobotPose();
updateRobotPath();
updateFollowCamera();
}
}
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.visible = navigationInitialPoseSet;
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];
let pathChanged = false;
if (!last || last.distanceTo(p) > 0.03) {
robotPathPoints.push(p);
if (robotPathPoints.length > 3000) robotPathPoints.shift();
pathChanged = true;
}
if (!pathVisible) return;
if (!robotPathLine) {
robotPathLine = new THREE.Line(
new THREE.BufferGeometry(),
new THREE.LineBasicMaterial({ color: 0xffcc00, linewidth: 2 })
);
sdfRoot.add(robotPathLine);
pathChanged = true;
}
if (!pathChanged) return;
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 clearRobotPath() {
robotPathPoints.length = 0;
if (!robotPathLine) return;
sdfRoot.remove(robotPathLine);
disposeObject(robotPathLine);
robotPathLine = null;
}
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, scanPose = (
NAV2_VIEW_MODE === 'navigation'
? latestSlamState?.scan_pose
: latestSlamState?.scan_pose_map
) || null) {
if (NAV2_VIEW_MODE === 'navigation' && !navigationInitialPoseSet) {
if (laserGroup) {
sdfRoot.remove(laserGroup);
disposeObject(laserGroup);
laserGroup = null;
}
return;
}
if (!scan?.ranges?.length) return;
if (laserGroup) {
sdfRoot.remove(laserGroup);
disposeObject(laserGroup);
}
const sourcePose = scanPose || currentRobotPose;
const renderPose = REAL_RUNTIME && scanPose
? { ...sourcePose, z: (sourcePose.z || 0) - MKMINI_LASER_RENDER_Z_OFFSET }
: sourcePose;
const origin = scanPose
? new THREE.Vector3(renderPose.x, renderPose.y, renderPose.z || 0)
: laserOriginFromPose(renderPose);
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(sourcePose, 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 = '#111520';
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) => {
const yaw = Number(grid.origin.yaw || 0);
const c = Math.cos(yaw);
const s = Math.sin(yaw);
const x = wx - grid.origin.x;
const y = wy - grid.origin.y;
const localX = c * x + s * y;
const localY = -s * x + c * y;
return {
x: dx + (localX / grid.resolution) * scale,
y: dy + (grid.height - localY / 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 scanFrame = state?.scan_pose_map || null;
const frameLen = Math.max(grid.resolution * 12, 0.18);
if (navigationInitialPoseSet && state?.scan?.ranges?.length && (state?.pose_map || state?.pose)) {
const sourceFrame = scanFrame || robotFrame;
const c = Math.cos(sourceFrame.yaw);
const s = Math.sin(sourceFrame.yaw);
const laserOrigin = scanFrame
? { x: sourceFrame.x, y: sourceFrame.y }
: {
x: sourceFrame.x + c * LASER_OFFSET_X - s * LASER_OFFSET_Y,
y: sourceFrame.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 = sourceFrame.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 (navigationInitialPoseSet && (state?.pose || state?.pose_map)) drawLink(odomFrame, robotFrame);
drawFrame(mapFrame, 'map');
drawFrame(odomFrame, 'odom');
if (navigationInitialPoseSet && (state?.pose || state?.pose_map)) drawFrame(robotFrame, 'robot');
return;
}
miniCtx.fillStyle = 'rgba(255,255,255,.68)';
miniCtx.font = '600 13px sans-serif';
miniCtx.textAlign = 'center';
miniCtx.textBaseline = 'middle';
miniCtx.fillText(
SHOW_POINT_CLOUD && state?.point_cloud
? '3D 포인트클라우드를 표시 중입니다.'
: '모듈을 연결해주세요.',
w / 2,
h / 2,
);
miniCtx.textAlign = 'start';
miniCtx.textBaseline = 'alphabetic';
}
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', async () => {
if (downloadMapButton.disabled) return;
downloadMapButton.disabled = true;
try {
const response = await fetch(`${BACKEND_URL}/save-map`, {
method: 'POST',
headers: { 'content-type': 'application/json' },
body: JSON.stringify({})
});
const payload = await response.json().catch(() => ({}));
if (!response.ok) throw new Error(payload.detail || payload.message || '맵 저장에 실패했습니다.');
window.parent.postMessage({ source: 'autonomy-slam-viewer', type: 'map-saved', map: payload.map }, '*');
} catch (error) {
window.parent.postMessage({ source: 'autonomy-slam-viewer', type: 'map-save-failed', message: error instanceof Error ? error.message : '맵 저장에 실패했습니다.' }, '*');
} finally {
downloadMapButton.disabled = !latestMiniMapGrid?.data?.length;
focusViewer();
}
});
pathToggleButton.addEventListener('click', () => {
setPathVisible(!pathVisible);
focusViewer();
});
goalToggleButton.addEventListener('click', () => {
if (goalToggleButton.disabled) return;
navigationGoalMode = !navigationGoalMode;
initialPoseMode = false;
obstaclePlacementMode = false;
hideObstaclePreview();
poseToggleButton.classList.remove('active');
obstacleToggleButton.classList.remove('active');
obstacleToggleButton.setAttribute('aria-pressed', 'false');
hideInitialPosePreview();
goalToggleButton.classList.toggle('active', navigationGoalMode);
if (!navigationGoalMode) hideNavigationGoalPreview();
focusViewer();
});
poseToggleButton.addEventListener('click', () => {
initialPoseMode = !initialPoseMode;
initialPoseDrag = null;
navigationGoalMode = false;
hideNavigationGoalPreview();
obstaclePlacementMode = false;
hideObstaclePreview();
goalToggleButton.classList.remove('active');
obstacleToggleButton.classList.remove('active');
obstacleToggleButton.setAttribute('aria-pressed', 'false');
poseToggleButton.classList.toggle('active', initialPoseMode);
if (!initialPoseMode) hideInitialPosePreview();
focusViewer();
});
obstacleToggleButton.addEventListener('click', () => {
if (obstaclePlacementMode) {
obstaclePlacementMode = false;
obstacleToggleButton.classList.remove('active');
obstacleToggleButton.setAttribute('aria-pressed', 'false');
clearRuntimeObstacles();
hideObstaclePreview();
window.parent.postMessage({ source:'autonomy-slam-viewer', type:'runtime-obstacle-clear' }, '*');
focusViewer();
return;
}
obstaclePlacementMode = !obstaclePlacementMode;
navigationGoalMode = false;
initialPoseMode = false;
hideNavigationGoalPreview();
hideInitialPosePreview();
obstaclePreviewTemplate = null;
goalToggleButton.classList.remove('active');
poseToggleButton.classList.remove('active');
obstacleToggleButton.classList.toggle('active', obstaclePlacementMode);
obstacleToggleButton.setAttribute('aria-pressed', String(obstaclePlacementMode));
focusViewer();
});
initToggleButton.addEventListener('click', async () => {
navigationGoalMode = false;
initialPoseMode = false;
obstaclePlacementMode = false;
goalToggleButton.classList.remove('active');
poseToggleButton.classList.remove('active');
obstacleToggleButton.classList.remove('active');
obstacleToggleButton.setAttribute('aria-pressed', 'false');
navigationInitialPoseSet = false;
syncGoalToggleAvailability();
navigationPlanSuppressed = true;
navigationGoalId = null;
clearNavigationGoalMarker();
hideNavigationGoalPreview();
clearRuntimeObstacles();
hideObstaclePreview();
setPathVisible(false);
clearRobotPath();
navigationPlanSignature = '';
if (navigationPlanLine) {
sdfRoot.remove(navigationPlanLine);
navigationPlanLine.geometry.dispose();
navigationPlanLine.material.dispose();
navigationPlanLine = null;
}
if (robotModel) robotModel.visible = false;
hideInitialPosePreview();
await fetch(`${BACKEND_URL}/cancel_navigation`, { method:'POST' }).catch(() => undefined);
window.parent.postMessage({ source:'autonomy-slam-viewer', type:'navigation-init' }, '*');
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 odomToMap(pose) {
if (!navigationMapToOdom) return pose;
const c = Math.cos(navigationMapToOdom.yaw || 0);
const s = Math.sin(navigationMapToOdom.yaw || 0);
return {
x: navigationMapToOdom.x + c * pose.x - s * pose.y,
y: navigationMapToOdom.y + s * pose.x + c * pose.y,
yaw: (pose.yaw || 0) + (navigationMapToOdom.yaw || 0),
};
}
function mapToOdom(pose) {
if (!navigationMapToOdom) return pose;
const yaw = navigationMapToOdom.yaw || 0;
const c = Math.cos(yaw);
const s = Math.sin(yaw);
const dx = pose.x - navigationMapToOdom.x;
const dy = pose.y - navigationMapToOdom.y;
return {
x: c * dx + s * dy,
y: -s * dx + c * dy,
yaw: (pose.yaw || 0) - yaw,
};
}
function placeNavigationGoalMarker(x, y, yaw = 0) {
navigationGoalPose = { x, y, yaw };
if (!navigationGoalMarker) {
const marker = new THREE.Group();
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;
}
updateNavigationGoalMarker();
}
function updateNavigationGoalMarker() {
if (!navigationGoalMarker || !navigationGoalPose) return;
const markerPose = NAV2_VIEW_MODE === 'navigation'
? mapToOdom(navigationGoalPose)
: navigationGoalPose;
navigationGoalMarker.position.set(markerPose.x, markerPose.y, 0.05);
navigationGoalMarker.rotation.z = markerPose.yaw;
}
function setNavigationGoalPreview(pose) {
if (!navigationGoalPreview) {
navigationGoalPreview = new THREE.Mesh(
new THREE.TorusGeometry(0.26, 0.025, 8, 36),
new THREE.MeshBasicMaterial({ color:0x39ff88, transparent:true, opacity:0.72, depthWrite:false })
);
sdfRoot.add(navigationGoalPreview);
}
const odomPose = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(pose) : pose;
navigationGoalPreview.position.set(odomPose.x, odomPose.y, 0.06);
navigationGoalPreview.rotation.z = odomPose.yaw || 0;
navigationGoalPreview.visible = true;
}
function hideNavigationGoalPreview() {
if (navigationGoalPreview) navigationGoalPreview.visible = false;
}
function addRuntimeObstacle(obstacle) {
const mesh = new THREE.Mesh(
new THREE.BoxGeometry(obstacle.length, obstacle.depth, obstacle.height),
new THREE.MeshStandardMaterial({ color:0xd97706, roughness:0.78, metalness:0.03 })
);
mesh.position.set(obstacle.x, obstacle.y, obstacle.height / 2);
mesh.rotation.z = obstacle.yaw;
mesh.userData.obstacleId = obstacle.id;
runtimeObstacleObjects.add(mesh);
}
function clearRuntimeObstacles() {
clearLayoutObjects(runtimeObstacleObjects);
}
function createObstaclePreviewTemplate() {
return {
yaw: (Math.floor(Math.random() * 4) * Math.PI) / 2,
length: 0.7 + Math.random() * 0.9,
depth: 0.16,
height: 0.52,
};
}
function setObstaclePreview(pose) {
if (!obstaclePreviewTemplate) obstaclePreviewTemplate = createObstaclePreviewTemplate();
if (!obstaclePreview) {
obstaclePreview = new THREE.Mesh(
new THREE.BoxGeometry(obstaclePreviewTemplate.length, obstaclePreviewTemplate.depth, obstaclePreviewTemplate.height),
new THREE.MeshStandardMaterial({ color:0xf59e0b, transparent:true, opacity:0.45, depthWrite:false })
);
sdfRoot.add(obstaclePreview);
}
const odomPose = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(pose) : pose;
obstaclePreview.position.set(odomPose.x, odomPose.y, obstaclePreviewTemplate.height / 2);
obstaclePreview.rotation.z = obstaclePreviewTemplate.yaw;
obstaclePreview.visible = true;
}
function hideObstaclePreview() {
if (obstaclePreview) obstaclePreview.visible = false;
}
function updateNavigationPlan(plan) {
if (navigationPlanSuppressed) return;
const poses = Array.isArray(plan?.poses) ? plan.poses : [];
const signature = poses.map((pose) => `${Number(pose.x).toFixed(3)},${Number(pose.y).toFixed(3)}`).join('|');
if (signature === navigationPlanSignature) return;
navigationPlanSignature = signature;
if (navigationPlanLine) {
sdfRoot.remove(navigationPlanLine);
navigationPlanLine.geometry.dispose();
navigationPlanLine.material.dispose();
navigationPlanLine = null;
}
if (poses.length < 2) return;
const points = poses.map((pose) => {
const odomPose = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(pose) : pose;
return new THREE.Vector3(odomPose.x, odomPose.y, 0.07);
});
navigationPlanLine = new THREE.Line(
new THREE.BufferGeometry().setFromPoints(points),
new THREE.LineBasicMaterial({ color:0x60a5fa, transparent:true, opacity:0.95 })
);
sdfRoot.add(navigationPlanLine);
}
function clearNavigationGoalMarker() {
if (!navigationGoalMarker) return;
sdfRoot.remove(navigationGoalMarker);
disposeObject(navigationGoalMarker);
navigationGoalMarker = null;
navigationGoalPose = null;
}
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);
const mapPoint = NAV2_VIEW_MODE === 'navigation'
? odomToMap({ x: sdfPoint.x, y: sdfPoint.y, yaw: currentRobotPose.yaw || 0 })
: sdfPoint;
return {
x: mapPoint.x,
y: mapPoint.y,
yaw: mapPoint.yaw || 0,
};
}
function showInitialPosePreview(pose) {
if (!posePreviewModel) return;
const scenePose = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(pose) : pose;
posePreviewModel.position.set(scenePose.x, scenePose.y, 0.01);
posePreviewModel.rotation.set(0, 0, scenePose.yaw || 0);
posePreviewModel.visible = true;
if (!initialPoseDrag) {
if (initialPoseDirectionArrow) initialPoseDirectionArrow.visible = false;
return;
}
if (!initialPoseDirectionArrow) {
initialPoseDirectionArrow = new THREE.ArrowHelper(
new THREE.Vector3(1, 0, 0),
new THREE.Vector3(),
0.72,
0xfacc15,
0.18,
0.11,
);
initialPoseDirectionArrow.name = 'initial-pose-direction';
initialPoseDirectionArrow.renderOrder = 4;
sdfRoot.add(initialPoseDirectionArrow);
}
initialPoseDirectionArrow.position.set(scenePose.x, scenePose.y, 0.12);
initialPoseDirectionArrow.setDirection(
new THREE.Vector3(Math.cos(scenePose.yaw || 0), Math.sin(scenePose.yaw || 0), 0),
);
initialPoseDirectionArrow.visible = true;
}
function hideInitialPosePreview() {
if (posePreviewModel) posePreviewModel.visible = false;
if (initialPoseDirectionArrow) initialPoseDirectionArrow.visible = false;
}
async function sendNavigationGoal(goal) {
// Nav2 cancellation is asynchronous. Confirm any earlier goal is canceled
// before accepting the next one so it cannot keep driving to the old point.
const cancelResponse = await fetch(`${BACKEND_URL}/cancel_navigation`, { method:'POST' });
if (!cancelResponse.ok) {
const message = await cancelResponse.text().catch(() => '');
throw new Error(message || '기존 목표를 취소하지 못했습니다.');
}
navigationGoalId = null;
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');
}
const result = await response.json().catch(() => ({ ok:true }));
navigationGoalId = result.goal_id || null;
return result;
}
async function sendInitialPose(pose) {
if (REAL_RUNTIME) {
const response = await fetch(`${BACKEND_URL}/initialpose`, {
method:'POST',
headers:{ 'Content-Type':'application/json' },
body:JSON.stringify({ ...pose, frame_id:'map' }),
});
if (!response.ok) throw new Error(await response.text().catch(() => '') || 'initial pose failed');
navigationInitialPoseSet = true;
syncGoalToggleAvailability();
navigationPlanSuppressed = false;
if (robotModel) robotModel.visible = true;
hideInitialPosePreview();
return;
}
const gazeboPose = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(pose) : pose;
const requestId = `initial-pose-${Date.now()}-${++initialPoseRequestSequence}`;
await new Promise((resolve, reject) => {
const timeout = window.setTimeout(() => {
window.removeEventListener('message', onMessage);
reject(new Error('Gazebo 로봇 위치 적용 시간이 초과되었습니다.'));
}, 5000);
const onMessage = (event) => {
const result = event.data;
if (result?.source !== 'autonomy-lms-shell' || result?.type !== 'initial-pose-applied' || result?.requestId !== requestId) return;
window.clearTimeout(timeout);
window.removeEventListener('message', onMessage);
if (result.ok) resolve();
else reject(new Error(result.message || 'Gazebo 로봇 위치 적용에 실패했습니다.'));
};
window.addEventListener('message', onMessage);
window.parent.postMessage({
source: 'autonomy-slam-viewer',
type: 'initial-pose-selected',
requestId,
pose: {
x: gazeboPose.x,
y: gazeboPose.y,
yaw: gazeboPose.yaw || 0,
},
}, '*');
});
await new Promise((resolve) => window.setTimeout(resolve, 150));
const response = await fetch(`${BACKEND_URL}/initialpose`, {
method:'POST',
headers:{ 'Content-Type':'application/json' },
body:JSON.stringify({ ...pose, frame_id:'map' }),
});
if (!response.ok) throw new Error(await response.text().catch(() => '') || 'initial pose failed');
navigationInitialPoseSet = true;
syncGoalToggleAvailability();
navigationPlanSuppressed = false;
if (robotModel) robotModel.visible = true;
hideInitialPosePreview();
}
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('mousemove', (event) => {
if (!initialPoseMode && !navigationGoalMode && !obstaclePlacementMode) return;
if (initialPoseDrag) return;
const pose = pickNavigationGoal(event);
if (!pose) return;
if (initialPoseMode) showInitialPosePreview(pose);
if (navigationGoalMode) setNavigationGoalPreview(pose);
if (obstaclePlacementMode) setObstaclePreview(pose);
});
renderer.domElement.addEventListener('pointerdown', (event) => {
if (EDITOR_MODE || !initialPoseMode || event.button !== 0) return;
const pose = pickNavigationGoal(event);
if (!pose) return;
initialPoseDrag = { ...pose };
controls.enabled = false;
renderer.domElement.setPointerCapture?.(event.pointerId);
showInitialPosePreview(initialPoseDrag);
event.preventDefault();
event.stopImmediatePropagation();
}, true);
renderer.domElement.addEventListener('pointermove', (event) => {
if (!initialPoseDrag) return;
const direction = pickNavigationGoal(event);
if (!direction) return;
const dx = direction.x - initialPoseDrag.x;
const dy = direction.y - initialPoseDrag.y;
if (Math.hypot(dx, dy) > 0.05) initialPoseDrag.yaw = Math.atan2(dy, dx);
showInitialPosePreview(initialPoseDrag);
event.preventDefault();
event.stopImmediatePropagation();
}, true);
renderer.domElement.addEventListener('pointerup', async (event) => {
if (!initialPoseDrag) return;
const pose = { ...initialPoseDrag };
initialPoseDrag = null;
controls.enabled = true;
renderer.domElement.releasePointerCapture?.(event.pointerId);
initialPoseMode = false;
poseToggleButton.classList.remove('active');
hideInitialPosePreview();
event.preventDefault();
event.stopImmediatePropagation();
try {
await sendInitialPose(pose);
} catch (error) {
console.warn('initial pose failed:', error);
}
}, true);
renderer.domElement.addEventListener('pointercancel', () => {
if (!initialPoseDrag) return;
initialPoseDrag = null;
controls.enabled = true;
hideInitialPosePreview();
}, true);
renderer.domElement.addEventListener('click', async (event) => {
if (EDITOR_MODE) {
const rect = renderer.domElement.getBoundingClientRect();
editorRaycaster.setFromCamera(new THREE.Vector2(
((event.clientX - rect.left) / rect.width) * 2 - 1,
-((event.clientY - rect.top) / rect.height) * 2 + 1,
), camera);
const hit = editorRaycaster.intersectObjects(editorObjects.children, false)[0];
editorSelectedId = hit?.object?.userData?.objectId || null;
attachEditorTransform();
window.parent.postMessage({ source:'autonomy-world-editor', type:'select-object', id: editorSelectedId }, '*');
return;
}
if (!navigationGoalMode && !initialPoseMode && !obstaclePlacementMode) return;
event.preventDefault();
event.stopPropagation();
const goal = pickNavigationGoal(event);
if (!goal) return;
try {
if (obstaclePlacementMode) {
const sdfPoint = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(goal) : goal;
const obstacle = {
id: `runtime-wall-${Date.now()}-${Math.random().toString(36).slice(2, 7)}`,
x: sdfPoint.x,
y: sdfPoint.y,
...(obstaclePreviewTemplate ?? createObstaclePreviewTemplate()),
};
window.parent.postMessage({ source:'autonomy-slam-viewer', type:'runtime-obstacle-add', obstacle }, '*');
addRuntimeObstacle(obstacle);
obstaclePreviewTemplate = null;
setObstaclePreview(goal);
return;
}
if (initialPoseMode) return;
await sendNavigationGoal(goal);
navigationGoalMode = false;
goalToggleButton.classList.remove('active');
hideNavigationGoalPreview();
} catch (error) {
console.warn('navigation goal failed:', error);
}
});
function applySlamState(state) {
latestSlamState = state;
if (
navigationGoalMarker
&& navigationGoalId
&& state?.navigation?.goal_id === navigationGoalId
&& state.navigation.status === 'succeeded'
) {
clearNavigationGoalMarker();
navigationGoalId = null;
}
if (state?.pose || state?.pose_3d || state?.scan || state?.map || state?.point_cloud) markVisualizationResponseReceived();
if (NAV2_VIEW_MODE === 'navigation' && state?.tf?.map_to_odom) {
navigationMapToOdom = state.tf.map_to_odom;
}
updateNavigationPlan(state?.plan);
// Navigation is rendered in odom so the map, robot, path and goal share
// one scene frame. SLAM keeps its map-frame pose for the live map view.
const visualizationPose = SHOW_POINT_CLOUD
? (state?.pose_3d || state?.pose)
: (NAV2_VIEW_MODE === 'navigation' ? state?.pose : (state?.pose_map || state?.pose));
if (visualizationPose) setRobotPose(visualizationPose);
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();
});
connectControlSocket();
}
function connectControlSocket() {
if (!CONTROL_WS) return;
controlSocket = new WebSocket(CONTROL_WS);
controlSocket.addEventListener('close', () => {
setTimeout(connectControlSocket, 1000);
});
controlSocket.addEventListener('error', () => {
controlSocket.close();
});
}
let maxLinear = 0.22;
let maxAngular = 2.84;
let activeMoveKey = null;
let teleopInterval = null;
let singleMoveStopTimer = null;
const TELEOP_COMMAND_INTERVAL_MS = 120;
const SINGLE_MOVE_DURATION_MS = 120;
const LATCHED_ROTATION_SPEED_SCALE = 0.25;
let cmdVelRequestInFlight = false;
let queuedCmdVel = 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 flushQueuedCmdVel() {
if (cmdVelRequestInFlight || !queuedCmdVel) return;
const command = queuedCmdVel;
queuedCmdVel = null;
cmdVelRequestInFlight = true;
const controller = new AbortController();
const timeout = window.setTimeout(() => controller.abort(), 800);
fetch(`${BACKEND_URL}/cmd_vel`, {
method:'POST',
headers:{ 'Content-Type':'application/json' },
body:JSON.stringify(command),
signal:controller.signal,
})
.catch((error) => {
if (error?.name !== 'AbortError') console.warn(error);
})
.finally(() => {
window.clearTimeout(timeout);
cmdVelRequestInFlight = false;
flushQueuedCmdVel();
});
}
function sendCmdVel(linear, angular) {
const payload = { type:'cmd_vel', linear, angular };
if (controlSocket?.readyState === WebSocket.OPEN) {
controlSocket.send(JSON.stringify(payload));
return;
}
if (!DIRECT_BRIDGE && slamSocket?.readyState === WebSocket.OPEN) {
slamSocket.send(JSON.stringify(payload));
return;
}
queuedCmdVel = { linear, angular };
flushQueuedCmdVel();
}
function stopRobot() {
if (teleopInterval) {
window.clearInterval(teleopInterval);
teleopInterval = null;
}
if (singleMoveStopTimer) {
window.clearTimeout(singleMoveStopTimer);
singleMoveStopTimer = null;
}
sendCmdVel(0, 0);
}
function sendActiveMoveCommand() {
if (!activeMoveKey) return;
const [linearSign, angularSign] = moveBindings[activeMoveKey];
if (linearSign === 0 && angularSign === 0) return;
const angularSpeed = linearSign === 0
? maxAngular * LATCHED_ROTATION_SPEED_SCALE
: maxAngular;
sendCmdVel(linearSign * maxLinear, angularSign * angularSpeed);
}
function startActiveMoveCommand() {
sendActiveMoveCommand();
if (teleopInterval) return;
teleopInterval = window.setInterval(sendActiveMoveCommand, TELEOP_COMMAND_INTERVAL_MS);
}
function sendSingleMoveCommand(key) {
const [linearSign, angularSign] = moveBindings[key];
activeMoveKey = null;
stopRobot();
sendCmdVel(linearSign * maxLinear, angularSign * maxAngular);
singleMoveStopTimer = window.setTimeout(() => {
singleMoveStopTimer = null;
sendCmdVel(0, 0);
}, SINGLE_MOVE_DURATION_MS);
}
window.addEventListener('keydown', (event) => {
if (!canUseTeleop(event)) return;
const key = event.key.toLowerCase();
if (event.repeat) {
if (moveBindings[key]) event.preventDefault();
return;
}
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();
if (key === 'k') {
activeMoveKey = null;
stopRobot();
return;
}
if (!['i', ',', 'j', 'l'].includes(key)) {
sendSingleMoveCommand(key);
return;
}
activeMoveKey = key;
startActiveMoveCommand();
});
window.addEventListener('keyup', (event) => {
if (!canUseTeleop(event)) return;
const key = event.key.toLowerCase();
if (moveBindings[key]) event.preventDefault();
});
window.addEventListener('blur', () => {
if (!activeMoveKey) return;
activeMoveKey = null;
stopRobot();
});
// A static Gazebo model can contain assets that the browser cannot render.
// Keep the ROS stream available even when the decorative world mesh fails.
const worldSceneTasks = CUSTOM_WORLD || REAL_RUNTIME
? []
: [buildFromSdf().catch((error) => console.warn('world model render failed', error))];
Promise.all(EDITOR_MODE
? worldSceneTasks
: [...worldSceneTasks, (REAL_RUNTIME ? loadMkMiniModel : loadBurgerModel)({ x:-2.0, y:-0.5, z:0.01, roll:0, pitch:0, yaw:0 })]
).then(() => {
if (EDITOR_MODE) {
window.parent.postMessage({ source:'autonomy-world-editor', type:'ready' }, '*');
return;
}
void refreshRuntimeLayout();
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>