1343 lines
44 KiB
HTML
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>
|