Files
lms-fe/src/renderer/public/slam-viewer/index.html
T

3421 lines
123 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:240px;
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;
}
#cameraPreviewWrap {
position:absolute;
right:16px;
bottom:256px;
z-index:10;
display:none;
width:260px;
border:1px solid rgba(0,0,0,.45);
background:#111520;
box-shadow:0 10px 26px rgba(0,0,0,.16);
}
#cameraPreviewWrap.is-visible { display:block; }
#cameraPreviewWrap.is-collapsed { background:transparent; border-color:transparent; box-shadow:none; }
#cameraPreview {
display:block;
width:260px;
aspect-ratio:16 / 9;
object-fit:cover;
background:#111520;
}
#cameraPreviewWrap.is-collapsed #cameraPreview { display:none; }
#cameraPreviewToggle {
position:absolute;
right:0;
top:-28px;
display:grid;
width:30px;
height:28px;
place-items:center;
padding:0;
border:1px solid rgba(255,255,255,.22);
background:rgba(17,24,39,.42);
color:rgba(255,255,255,.9);
font:11px sans-serif;
font-weight:800;
cursor:pointer;
backdrop-filter:blur(8px);
box-shadow:0 10px 28px rgba(0,0,0,.16);
}
#cameraPreviewToggle:hover { color:#fff; background:rgba(255,255,255,.06); }
#cameraPreviewChevron { font-size:13px; line-height:1; }
#cameraPreviewWrap.is-collapsed #cameraPreviewChevron { transform:rotate(180deg); }
#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);
}
#rosTopicStatusWrap {
position:absolute;
left:16px;
bottom:54px;
z-index:10;
width:178px;
}
#rosTopicStatus {
overflow:hidden;
border:1px solid rgba(255,255,255,.24);
background:rgba(17,24,39,.48);
color:rgba(255,255,255,.88);
font:11px sans-serif;
backdrop-filter:blur(8px);
box-shadow:0 10px 28px rgba(0,0,0,.16);
}
#rosTopicStatusToggle {
position:absolute;
left:0;
top:-28px;
display:grid;
width:30px;
height:28px;
place-items:center;
padding:0;
border:1px solid rgba(255,255,255,.22);
background:rgba(17,24,39,.42);
color:rgba(255,255,255,.9);
font:11px sans-serif;
font-weight:800;
cursor:pointer;
backdrop-filter:blur(8px);
box-shadow:0 10px 28px rgba(0,0,0,.16);
}
#rosTopicStatusToggle:hover { color:#fff; background:rgba(255,255,255,.06); }
#rosTopicStatusWrap.is-collapsed #rosTopicStatus { display:none; }
#rosTopicStatusChevron { font-size:13px; line-height:1; }
#rosTopicStatusWrap.is-collapsed #rosTopicStatusChevron { transform:rotate(180deg); }
#rosTopicStatusTitle {
min-height:30px;
display:flex;
align-items:center;
padding:0 9px;
border-bottom:1px solid rgba(255,255,255,.14);
font-weight:800;
}
#rosTopicStatusList { padding:4px 0; }
.ros-topic-row {
display:flex;
align-items:center;
justify-content:space-between;
gap:8px;
min-height:23px;
padding:0 9px;
}
.ros-topic-name { overflow:hidden; text-overflow:ellipsis; white-space:nowrap; }
.ros-topic-state { flex:0 0 auto; font-size:12px; font-weight:900; }
.ros-topic-state.is-ok { color:#5eead4; }
.ros-topic-state.is-error { color:#fca5a5; }
#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;
}
#mapSaveOverlay {
position:fixed;
inset:0;
z-index:40;
display:flex;
align-items:center;
justify-content:center;
background:rgba(5,8,16,.46);
backdrop-filter:blur(2px);
opacity:0;
pointer-events:none;
transition:opacity .16s ease;
}
#mapSaveOverlay.is-visible {
opacity:1;
pointer-events:auto;
}
.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,
#sensorToggle,
#poseToggle,
#goalToggle,
#obstacleToggle,
#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,
#sensorToggle,
#poseToggle,
#goalToggle,
#obstacleToggle {
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,
#sensorToggle: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;
}
#poseToggle:disabled {
opacity:.32;
cursor:not-allowed;
}
#pathToggle.active {
background:rgba(37,99,235,.62);
border-color:rgba(147,197,253,.55);
color:#fff;
}
#sensorToggle.active {
background:rgba(14,116,144,.62);
border-color:rgba(103,232,249,.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 #cameraPreviewWrap,
.editor-mode #keyGuide,
.editor-mode #rosTopicStatusWrap {
display:none;
}
.nav2-slam #goalToggle, .nav2-slam #poseToggle { display:none; }
.nav2-navigation #downloadWrap { display:none; }
.nav2-navigation #topLeftControls { display:block; }
.nav2-navigation #keyGuide { display:none; }
.nav2-navigation #rosTopicStatusWrap { bottom:16px; }
.real-runtime #topLeftControls { display:none; }
.real-runtime #keyGuide { 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="sensorToggle" title="센서 표시" aria-label="센서 표시" aria-pressed="false">Sensor</button>
<button id="poseToggle" title="초기 위치 지정" aria-label="초기 위치 지정">Pose</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="cameraPreviewWrap">
<button id="cameraPreviewToggle" type="button" title="카메라 화면 접기/펼치기" aria-label="카메라 화면 접기" aria-expanded="true"><span id="cameraPreviewChevron" aria-hidden="true">▾</span></button>
<img id="cameraPreview" alt="실시간 카메라 화면" />
</div>
<div id="keyGuide">
<div>i 전진 · , 후진 · j/l 회전 · k 정지</div>
</div>
<div id="rosTopicStatusWrap">
<button id="rosTopicStatusToggle" type="button" title="ROS 상태 접기/펼치기" aria-label="ROS 상태 접기" aria-expanded="true">
<span id="rosTopicStatusChevron" aria-hidden="true">▾</span>
</button>
<aside id="rosTopicStatus" aria-label="ROS 상태">
<div id="rosTopicStatusTitle">ROS 상태</div>
<div id="rosTopicStatusList"></div>
</aside>
</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>
<div id="mapSaveOverlay" role="status" aria-live="polite" aria-hidden="true">
<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 ROBOT_PLATFORM = (params.get('platform') || '').trim().toLowerCase();
// Keep the prior real-platform default when an older LMS URL has no platform.
const USE_MKMINI_MODEL = ROBOT_PLATFORM === 'mkmini' || (!ROBOT_PLATFORM && REAL_RUNTIME);
const SHOW_POINT_CLOUD = params.get('pointCloud') === '1';
const SHOW_SENSOR_POINT_CLOUD = REAL_RUNTIME && SHOW_POINT_CLOUD;
const SENSOR_POINT_CLOUD_MAX_AGE_MS = 3_000;
// Camera preview is kept for the RGB test module, but is paused for now so
// regular visualization sessions never poll `/camera-frame`.
const ENABLE_CAMERA_PREVIEW = false;
const CUSTOM_WORLD = !REAL_RUNTIME && viewerMapKey === 'nav2-custom';
const SAVED_GAZEBO_MAP_WORLD = !REAL_RUNTIME && viewerMapKey === 'nav2-saved-gazebo-map';
const activeWorldModel = viewerMapKey === 'nav2-turtlebot3-house' ? 'turtlebot3_house' : 'turtlebot3_world';
const MKMINI_WORLD_SINGLE_PILLAR = USE_MKMINI_MODEL && activeWorldModel === 'turtlebot3_world';
const MKMINI_RENDER_LENGTH = 0.84;
const MKMINI_RENDER_WIDTH = 0.61;
const MKMINI_RENDER_HEIGHT = 0.6575;
const MKMINI_WHEELBASE_METERS = 0.6;
const MKMINI_MAX_STEERING_RADIANS = 0.52;
const DEFAULT_SIMULATION_SPAWN = USE_MKMINI_MODEL
? { x: -1.4, y: -0.5 }
: { x: -2.0, y: -0.5 };
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 cameraPreviewWrap = document.getElementById('cameraPreviewWrap');
const cameraPreview = document.getElementById('cameraPreview');
const cameraPreviewToggle = document.getElementById('cameraPreviewToggle');
const responseWaitOverlay = document.getElementById('responseWaitOverlay');
const mapSaveOverlay = document.getElementById('mapSaveOverlay');
const downloadMapButton = document.getElementById('downloadMap');
const pathToggleButton = document.getElementById('pathToggle');
const sensorToggleButton = document.getElementById('sensorToggle');
const poseToggleButton = document.getElementById('poseToggle');
const goalToggleButton = document.getElementById('goalToggle');
const obstacleToggleButton = document.getElementById('obstacleToggle');
const cameraPresetSelect = document.getElementById('cameraPresetSelect');
const rosTopicStatusWrap = document.getElementById('rosTopicStatusWrap');
const rosTopicStatus = document.getElementById('rosTopicStatus');
const rosTopicStatusToggle = document.getElementById('rosTopicStatusToggle');
const rosTopicStatusList = document.getElementById('rosTopicStatusList');
const EDITOR_MODE = params.get('editor') === '1';
const NAV2_VIEW_MODE = params.get('mode') === 'navigation' ? 'navigation' : 'slam';
// A navigation session must keep the selected saved map as its fixed
// reference. The runtime /map topic may later emit a partial local update.
const LOCK_NAVIGATION_MAP = REAL_RUNTIME && NAV2_VIEW_MODE === 'navigation';
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;
const ROS_TOPIC_STATUS_KEYS = NAV2_VIEW_MODE === 'navigation'
? [
...(SHOW_POINT_CLOUD ? ['sensor_point_cloud', 'point_cloud'] : ['scan']),
'odom', 'map', 'plan', 'cmd_vel', 'navigate_to_pose',
]
: SHOW_POINT_CLOUD
? ['sensor_point_cloud', 'point_cloud', 'odom', 'map']
: ['scan', 'odom', 'map'];
const ROS_TOPIC_STATUS_FALLBACK_NAMES = {
scan: '/scan',
odom: '/odom',
map: '/map',
amcl_pose: '/amcl_pose',
plan: '/plan',
cmd_vel: '/cmd_vel',
navigate_to_pose: '/navigate_to_pose',
point_cloud: '/cloud_map',
sensor_point_cloud: '/lidar_filtered',
};
function updateRosTopicStatus(topicStatus = {}) {
if (!rosTopicStatusList) return;
rosTopicStatusList.replaceChildren(...ROS_TOPIC_STATUS_KEYS.map((key) => {
const status = topicStatus?.[key] ?? {};
const row = document.createElement('div');
row.className = 'ros-topic-row';
const name = document.createElement('span');
name.className = 'ros-topic-name';
name.textContent = status.name || ROS_TOPIC_STATUS_FALLBACK_NAMES[key] || key;
const indicator = document.createElement('span');
const available = status.available === true;
indicator.className = `ros-topic-state ${available ? 'is-ok' : 'is-error'}`;
indicator.textContent = available ? 'O' : 'X';
row.append(name, indicator);
return row;
}));
// Do not leave a previous lidar frame on screen after the bridge has marked
// its source stale. A stale point cloud looks like a map/laser transform bug.
if (Object.keys(topicStatus).length && topicStatus?.sensor_point_cloud?.available !== true) {
clearSensorPointCloud();
}
}
rosTopicStatusToggle?.addEventListener('click', () => {
const collapsed = rosTopicStatusWrap?.classList.toggle('is-collapsed') ?? false;
rosTopicStatusToggle.setAttribute('aria-expanded', String(!collapsed));
rosTopicStatusToggle.setAttribute('aria-label', collapsed ? 'ROS 상태 펼치기' : 'ROS 상태 접기');
});
updateRosTopicStatus();
function markVisualizationResponseReceived() {
if (visualizationResponseReceived) return;
visualizationResponseReceived = true;
responseWaitOverlay?.classList.add('is-hidden');
}
function setMapSavePending(isPending) {
mapSaveOverlay?.classList.toggle('is-visible', isPending);
mapSaveOverlay?.setAttribute('aria-hidden', String(!isPending));
}
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', (event) => {
event.preventDefault();
if (!obstaclePlacementMode) return;
obstaclePlacementMode = false;
obstacleToggleButton.classList.remove('active');
obstacleToggleButton.setAttribute('aria-pressed', 'false');
hideObstaclePreview();
focusViewer();
});
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);
// Gazebo world geometry is authored in `odom`. Keep it separate from the
// map-frame robot, scan, route and occupancy-map layers.
const gazeboWorldRoot = new THREE.Group();
gazeboWorldRoot.name = 'gazebo-world';
sdfRoot.add(gazeboWorldRoot);
const editorObjects = new THREE.Group();
sdfRoot.add(editorObjects);
const editorBoundaryObjects = new THREE.Group();
sdfRoot.add(editorBoundaryObjects);
const runtimeLayoutObjects = new THREE.Group();
gazeboWorldRoot.add(runtimeLayoutObjects);
const navigationMapRoot = new THREE.Group();
navigationMapRoot.name = 'navigation-occupancy-map';
sdfRoot.add(navigationMapRoot);
const pointCloudGeometry = SHOW_POINT_CLOUD ? new THREE.BufferGeometry() : null;
let pointCloudLayer = null;
let hasGlobalMapPointCloud = false;
// The live sensor point cloud belongs to the 3D view. 2D SLAM renders only
// LaserScan and must never poll the point-cloud endpoint.
const sensorPointCloudGeometry = SHOW_SENSOR_POINT_CLOUD ? new THREE.BufferGeometry() : null;
let sensorPointCloudLayer = null;
if (pointCloudGeometry) {
pointCloudLayer = new THREE.Points(
pointCloudGeometry,
new THREE.PointsMaterial({
color: 0x4fc3f7,
size: 0.035,
sizeAttenuation: true,
transparent: true,
opacity: 0.78,
depthWrite: false,
}),
);
pointCloudLayer.name = 'slam-point-cloud';
pointCloudLayer.renderOrder = 2;
sdfRoot.add(pointCloudLayer);
}
if (sensorPointCloudGeometry) {
sensorPointCloudLayer = new THREE.Points(
sensorPointCloudGeometry,
new THREE.PointsMaterial({
color: 0xff5c35,
size: 0.028,
sizeAttenuation: true,
transparent: true,
opacity: 0.82,
depthWrite: false,
}),
);
sensorPointCloudLayer.name = 'live-sensor-point-cloud';
sensorPointCloudLayer.renderOrder = 3;
sdfRoot.add(sensorPointCloudLayer);
}
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, bounds:{ width:10, depth:10 }, objects:[] };
clearLayoutObjects(editorObjects);
clearLayoutObjects(editorBoundaryObjects);
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({ bounds: layout?.bounds || null, objects: 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);
});
const pendingSimulatorPoseRequests = new Map();
window.addEventListener('message', (event) => {
const payload = event.data;
if (payload?.source !== 'autonomy-studio' || payload.type !== 'simulator-pose-result') return;
const request = pendingSimulatorPoseRequests.get(payload.requestId);
if (!request) return;
pendingSimulatorPoseRequests.delete(payload.requestId);
window.clearTimeout(request.timeout);
if (payload.ok) request.resolve();
else request.reject(new Error(payload.message || '가제보 로봇 위치를 적용하지 못했습니다.'));
});
let robotModel = null;
let posePreviewModel = null;
let mkMiniFrontWheelPivots = [];
let targetMkMiniSteeringAngle = 0;
let currentMkMiniSteeringAngle = 0;
let targetMkMiniSteeringAngles = { left: 0, right: 0 };
let currentMkMiniSteeringAngles = { left: 0, right: 0 };
let lastMkMiniSteeringUpdateMs = null;
let lastMkMiniMotionSample = null;
let hasSimulatorMkMiniSteering = false;
let laserGroup = null;
let laserFillMesh = null;
let laserLineSegments = null;
let laserOpenSpaceLineSegments = null;
let laserFrontBeam = 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 navigationMapMesh = null;
let navigationMapTexture = null;
let navigationMapLocked = false;
let navigationMapFocused = false;
let latestScanMetadata = null;
let latestScanRanges = null;
let lastLaserRenderAtMs = 0;
let lastMiniMapRenderAtMs = 0;
let pendingMiniMapState = null;
let miniMapRenderTimer = null;
let pointCloudRequestInFlight = false;
let pointCloudRefreshTimer = null;
let savedMapPointCloudLoaded = false;
let sensorPointCloudRequestInFlight = false;
let sensorPointCloudRefreshTimer = null;
let cameraFrameRequestInFlight = false;
let cameraFrameRefreshTimer = null;
let cameraFrameObjectUrl = null;
let pathVisible = false;
// SLAM always renders its live scan, including the forward beam. Navigation
// keeps the sensor opt-in so a saved-map view starts cleanly.
let navigationSensorVisible = NAV2_VIEW_MODE !== 'navigation';
if (sensorPointCloudLayer) sensorPointCloudLayer.visible = navigationSensorVisible;
sensorToggleButton.classList.toggle('active', navigationSensorVisible);
sensorToggleButton.setAttribute('aria-pressed', String(navigationSensorVisible));
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 navigationLocalizationReady = NAV2_VIEW_MODE !== 'navigation';
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);
// A saved map, the remote room model, and AMCL's pose all use `map`.
// Keep every navigation scene in that frame. Rendering a local Gazebo session
// in `odom` made AMCL's map -> odom correction rotate and translate the map
// itself whenever an initial pose was set.
function navigationSceneUsesMapFrame() {
return NAV2_VIEW_MODE === 'navigation';
}
function localGazeboNavigationUsesOdom() {
return !REAL_RUNTIME && NAV2_VIEW_MODE === 'navigation';
}
function shouldRenderRobot() {
return hasRobotPose && (NAV2_VIEW_MODE !== 'navigation' || navigationInitialPoseSet);
}
function hasNavigationLocalization(state) {
const mapPose = state?.pose_3d || state?.pose_map;
return Boolean(mapPose && state?.tf?.map_to_odom);
}
function syncGoalToggleAvailability() {
const navigationReady = navigationInitialPoseSet || navigationLocalizationReady;
const goalDisabled = NAV2_VIEW_MODE === 'navigation' && !navigationReady;
const poseDisabled = NAV2_VIEW_MODE === 'navigation' && !REAL_RUNTIME && navigationReady;
goalToggleButton.disabled = goalDisabled;
poseToggleButton.disabled = poseDisabled;
poseToggleButton.title = poseDisabled
? '주행 시뮬레이션에서는 초기 위치를 한 번만 설정할 수 있습니다.'
: '로봇 초기 위치 설정';
if (goalDisabled) {
navigationGoalMode = false;
goalToggleButton.classList.remove('active');
hideNavigationGoalPreview();
}
if (poseDisabled) {
initialPoseMode = false;
poseToggleButton.classList.remove('active');
hideInitialPosePreview();
}
}
function updateNavigationLocalizationState(state) {
if (NAV2_VIEW_MODE !== 'navigation') return;
navigationLocalizationReady = Boolean(
hasNavigationLocalization(state)
&& state?.topic_status?.odom?.available === true
&& state?.topic_status?.map?.available === true
);
if (navigationLocalizationReady) navigationPlanSuppressed = false;
syncGoalToggleAvailability();
}
function restoreNavigationPoseFromState(state) {
if (NAV2_VIEW_MODE !== 'navigation' || navigationInitialPoseSet) return;
if (state?.initial_pose_set !== true) return;
if (!hasNavigationLocalization(state)) return;
const restoredPose = state?.pose_3d || state?.pose_map || state?.pose;
if (!restoredPose) return;
// The bridge remains alive when only the viewer is minimized or closed.
// Reuse its current ROS pose instead of making the user initialize again.
navigationInitialPoseSet = true;
navigationPlanSuppressed = false;
syncGoalToggleAvailability();
if (robotModel) robotModel.visible = true;
}
function resetNavigationPoseForNewSession(state) {
if (NAV2_VIEW_MODE !== 'navigation' || state?.initial_pose_set !== false || !navigationInitialPoseSet) return;
navigationInitialPoseSet = false;
navigationPlanSuppressed = true;
hasRobotPose = false;
clearNavigationGoalMarker();
navigationGoalId = null;
hideInitialPosePreview();
clearLaserScan();
if (robotModel) robotModel.visible = false;
syncGoalToggleAvailability();
}
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 applyWorldSdfPose(obj, pose) {
applySdfPose(obj, pose);
}
function isMkMiniWorldPillar(visual) {
return MKMINI_WORLD_SINGLE_PILLAR
&& /^(one|two|three)_/.test(visual.getAttribute('name') || '');
}
function shouldRenderWorldVisual(visual) {
// The MK-mini Gazebo world keeps only the centre post from the 3x3 pillar
// grid. Mirror that generated-world rule in the Three.js preview.
return !isMkMiniWorldPillar(visual) || visual.getAttribute('name') === 'two_two';
}
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 sourceLength = Number(getDirectText(cyl, 'length', '1')) || 1;
const length = isMkMiniWorldPillar(visual) && sourceLength === .5 ? 1.2 : sourceLength;
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';
applyWorldSdfPose(modelGroup, parsePose(getDirectText(model, 'pose', '0 0 0 0 0 0')));
gazeboWorldRoot.add(modelGroup);
let loaded = 0;
async function addModelVisuals(sdfModel, parent) {
const nestedModelGroup = new THREE.Group();
let directGeometryCount = 0;
nestedModelGroup.name = sdfModel.getAttribute('name') || 'model';
applyWorldSdfPose(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';
applyWorldSdfPose(linkGroup, parsePose(getDirectText(link, 'pose', '0 0 0 0 0 0')));
nestedModelGroup.add(linkGroup);
for (const visual of Array.from(link.querySelectorAll(':scope > visual'))) {
if (!shouldRenderWorldVisual(visual)) continue;
const visualGroup = new THREE.Group();
visualGroup.name = visual.getAttribute('name') || 'visual';
applyWorldSdfPose(visualGroup, parsePose(getDirectText(visual, 'pose', '0 0 0 0 0 0')));
if (isMkMiniWorldPillar(visual)) visualGroup.position.z = 0.6;
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 = shouldRenderRobot();
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());
// The GLB is rotated into the SDF coordinate system above. Its local axes
// then map to SDF as X=length, Z=width, Y=height. Normalize every axis to
// the same physical envelope used by the Gazebo collision model.
const scaleX = MKMINI_RENDER_LENGTH / Math.max(initialSize.x, 0.001);
const scaleY = MKMINI_RENDER_HEIGHT / Math.max(initialSize.z, 0.001);
const scaleZ = MKMINI_RENDER_WIDTH / Math.max(initialSize.y, 0.001);
model.scale.set(scaleX, scaleY, scaleZ);
model.updateMatrixWorld(true);
const normalizedBounds = new THREE.Box3().setFromObject(model);
const center = normalizedBounds.getCenter(new THREE.Vector3());
// Keep the wheels on SDF Z=0 after scaling the vehicle height.
model.position.set(-center.x, -center.y, -normalizedBounds.min.z);
model.traverse((child) => {
if (!child.isMesh) return;
child.castShadow = true;
child.receiveShadow = true;
});
mkMiniFrontWheelPivots = createMkMiniFrontWheelPivots(model);
robotGroup.add(model);
sdfRoot.add(robotGroup);
robotModel = robotGroup;
robotModel.visible = shouldRenderRobot();
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);
}
function createMkMiniFrontWheelPivots(model) {
const pivots = [];
model.updateMatrixWorld(true);
for (const [wheelName, side] of [['FrontWheel_L', 'left'], ['FrontWheel_R', 'right']]) {
const wheel = model.getObjectByName(wheelName);
if (!wheel) continue;
const bounds = new THREE.Box3().setFromObject(wheel);
const center = bounds.getCenter(new THREE.Vector3());
const pivot = new THREE.Group();
pivot.name = `${wheelName}_steering_pivot`;
model.add(pivot);
pivot.position.copy(model.worldToLocal(center));
pivot.updateMatrixWorld(true);
// The source GLB keeps the wheel vertices in model coordinates. Reparenting
// through a local pivot preserves their pose while giving each wheel a steer axis.
pivot.attach(wheel);
pivots.push({ pivot, side });
}
return pivots;
}
const LASER_HOLE_R = 0.14;
const LASER_RENDER_INTERVAL_MS = 100;
const LASER_OPEN_SPACE_RAY_STRIDE = 4;
const MINI_MAP_RENDER_INTERVAL_MS = 100;
const LASER_OFFSET_X = USE_MKMINI_MODEL ? 0.15 : -0.032;
const LASER_OFFSET_Y = 0;
// Fallback height when an explicit ROS scan pose is unavailable.
const LASER_Z = USE_MKMINI_MODEL ? 0.5 : 0.181;
const MKMINI_LASER_RENDER_BACKWARD_OFFSET = 0.03;
function setRobotPose(pose) {
if (!pose) return;
targetRobotPose = {
x: Number(pose.x ?? targetRobotPose.x),
y: Number(pose.y ?? targetRobotPose.y),
// Gazebo navigation is a ground-plane simulation. A saved 3D map can
// include an elevated RTAB-Map pose, but that must not lift the rendered
// vehicle after the learner supplies a 2D initial pose.
z: REAL_RUNTIME ? Number(pose.z ?? targetRobotPose.z) : 0,
roll: REAL_RUNTIME ? Number(pose.roll ?? targetRobotPose.roll) : 0,
pitch: REAL_RUNTIME ? Number(pose.pitch ?? targetRobotPose.pitch) : 0,
yaw: Number(pose.yaw ?? targetRobotPose.yaw),
};
updateMkMiniSteeringTarget(pose);
if (!hasRobotPose) {
currentRobotPose = { ...targetRobotPose };
hasRobotPose = true;
applyCurrentRobotPose();
} 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 applyInitialNavigationPose(pose) {
const x = Number(pose?.x);
const y = Number(pose?.y);
const yaw = Number(pose?.yaw ?? 0);
if (![x, y, yaw].every(Number.isFinite)) return;
// An initial pose is a teleport, not a movement. Snap the rendered robot to
// the selected map pose so stale odometry cannot animate it across the map
// while AMCL publishes the new map -> odom correction.
targetRobotPose = { ...targetRobotPose, x, y, z: 0, roll: 0, pitch: 0, yaw };
currentRobotPose = { ...targetRobotPose };
hasRobotPose = true;
lastMkMiniMotionSample = null;
applyCurrentRobotPose();
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;
// Raw odometry is valid before localization, but it has no map-frame
// meaning. Keep the vehicle hidden until the learner sets the first pose.
robotModel.visible = shouldRenderRobot();
robotModel.position.set(currentRobotPose.x, currentRobotPose.y, currentRobotPose.z);
robotModel.rotation.set(currentRobotPose.roll, currentRobotPose.pitch, currentRobotPose.yaw, 'XYZ');
}
function updateMkMiniSteeringTarget(pose) {
if (!USE_MKMINI_MODEL || !mkMiniFrontWheelPivots.length) return;
const simulatorLeftSteeringAngle = Number(pose?.steeringLeftAngle);
const simulatorRightSteeringAngle = Number(pose?.steeringRightAngle);
if (Number.isFinite(simulatorLeftSteeringAngle) && Number.isFinite(simulatorRightSteeringAngle)) {
hasSimulatorMkMiniSteering = true;
targetMkMiniSteeringAngles = {
left: THREE.MathUtils.clamp(simulatorLeftSteeringAngle, -MKMINI_MAX_STEERING_RADIANS, MKMINI_MAX_STEERING_RADIANS),
right: THREE.MathUtils.clamp(simulatorRightSteeringAngle, -MKMINI_MAX_STEERING_RADIANS, MKMINI_MAX_STEERING_RADIANS),
};
targetMkMiniSteeringAngle = (targetMkMiniSteeringAngles.left + targetMkMiniSteeringAngles.right) * 0.5;
return;
}
const simulatorSteeringAngle = Number(pose?.steeringAngle);
if (Number.isFinite(simulatorSteeringAngle)) {
hasSimulatorMkMiniSteering = true;
targetMkMiniSteeringAngle = THREE.MathUtils.clamp(
simulatorSteeringAngle,
-MKMINI_MAX_STEERING_RADIANS,
MKMINI_MAX_STEERING_RADIANS,
);
targetMkMiniSteeringAngles = mkMiniAckermannAngles(targetMkMiniSteeringAngle);
return;
}
hasSimulatorMkMiniSteering = false;
const motion = pose?.motion ?? pose;
let linear = Number(motion?.linear);
let angular = Number(motion?.angular);
const sample = {
x: Number(motion?.x),
y: Number(motion?.y),
yaw: Number(motion?.yaw),
t: Number(motion?.t),
};
// Some vehicle odometry streams keep twist.angular stale. Derive the turn
// direction from consecutive odom poses when a fresh movement sample exists.
if (
lastMkMiniMotionSample
&& Number.isFinite(sample.x)
&& Number.isFinite(sample.y)
&& Number.isFinite(sample.yaw)
&& Number.isFinite(sample.t)
) {
const elapsed = sample.t - lastMkMiniMotionSample.t;
const deltaX = sample.x - lastMkMiniMotionSample.x;
const deltaY = sample.y - lastMkMiniMotionSample.y;
const distance = Math.hypot(deltaX, deltaY);
if (elapsed > 0.01 && elapsed < 1.5 && distance > 0.002) {
const forwardDistance = (
deltaX * Math.cos(lastMkMiniMotionSample.yaw)
+ deltaY * Math.sin(lastMkMiniMotionSample.yaw)
);
linear = forwardDistance / elapsed;
angular = lerpAngle(lastMkMiniMotionSample.yaw, sample.yaw, 1) - lastMkMiniMotionSample.yaw;
angular /= elapsed;
}
}
if (
Number.isFinite(sample.x)
&& Number.isFinite(sample.y)
&& Number.isFinite(sample.yaw)
&& Number.isFinite(sample.t)
) {
lastMkMiniMotionSample = sample;
}
if (!Number.isFinite(linear) || !Number.isFinite(angular)) return;
targetMkMiniSteeringAngle = Math.abs(linear) < 0.03
? 0
: THREE.MathUtils.clamp(
Math.atan2(MKMINI_WHEELBASE_METERS * angular, linear),
-MKMINI_MAX_STEERING_RADIANS,
MKMINI_MAX_STEERING_RADIANS,
);
targetMkMiniSteeringAngles = mkMiniAckermannAngles(targetMkMiniSteeringAngle);
}
function mkMiniAckermannAngles(steeringAngle) {
if (Math.abs(steeringAngle) < 0.0001) return { left: 0, right: 0 };
const direction = Math.sign(steeringAngle);
const radius = MKMINI_GAZEBO_WHEELBASE_METERS / Math.tan(Math.abs(steeringAngle));
const halfTrack = 0.255;
const inner = Math.atan(MKMINI_GAZEBO_WHEELBASE_METERS / Math.max(0.01, radius - halfTrack));
const outer = Math.atan(MKMINI_GAZEBO_WHEELBASE_METERS / (radius + halfTrack));
return direction > 0 ? { left: inner, right: outer } : { left: -outer, right: -inner };
}
function updateMkMiniFrontWheelSteering() {
if (!mkMiniFrontWheelPivots.length) return;
const now = performance.now();
const elapsedSeconds = lastMkMiniSteeringUpdateMs === null
? 0
: Math.min((now - lastMkMiniSteeringUpdateMs) / 1000, 0.1);
lastMkMiniSteeringUpdateMs = now;
const alpha = 1 - Math.exp(-8 * elapsedSeconds);
currentMkMiniSteeringAngle += (targetMkMiniSteeringAngle - currentMkMiniSteeringAngle) * alpha;
for (const { pivot, side } of mkMiniFrontWheelPivots) {
// The asset is Z-up before its model-level X rotation, so its local Y axis
// becomes the viewer's vertical steering axis.
currentMkMiniSteeringAngles[side] += (targetMkMiniSteeringAngles[side] - currentMkMiniSteeringAngles[side]) * alpha;
pivot.rotation.y = currentMkMiniSteeringAngles[side];
}
}
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 ensureLaserScanLayer() {
if (laserGroup) {
laserGroup.visible = true;
return;
}
laserGroup = new THREE.Group();
laserGroup.name = 'live-laser-scan';
const fillGeometry = new THREE.BufferGeometry();
laserFillMesh = new THREE.Mesh(
fillGeometry,
new THREE.MeshBasicMaterial({
color: 0x005cff,
transparent: true,
opacity: 0.6,
side: THREE.DoubleSide,
depthWrite: false,
}),
);
laserFillMesh.frustumCulled = false;
const lineGeometry = new THREE.BufferGeometry();
laserLineSegments = new THREE.LineSegments(
lineGeometry,
new THREE.LineBasicMaterial({ color: 0x3366ff, transparent: true, opacity: 0.5 }),
);
laserLineSegments.frustumCulled = false;
const openSpaceGeometry = new THREE.BufferGeometry();
laserOpenSpaceLineSegments = new THREE.LineSegments(
openSpaceGeometry,
new THREE.LineBasicMaterial({ color: 0x6f9dff, transparent: true, opacity: 0.28 }),
);
laserOpenSpaceLineSegments.frustumCulled = false;
laserFrontBeam = new THREE.Mesh(
new THREE.CylinderGeometry(0.006, 0.006, 1, 16),
new THREE.MeshBasicMaterial({ color: 0x005cff }),
);
laserFrontBeam.frustumCulled = false;
laserGroup.add(laserFillMesh, laserLineSegments, laserOpenSpaceLineSegments, laserFrontBeam);
sdfRoot.add(laserGroup);
}
function updateLaserGeometry(geometry, values) {
let position = geometry.getAttribute('position');
if (!position || position.array.length < values.length) {
const capacity = Math.max(1024, Math.ceil(values.length * 1.25));
position = new THREE.BufferAttribute(new Float32Array(capacity), 3);
position.setUsage(THREE.DynamicDrawUsage);
geometry.setAttribute('position', position);
}
position.array.set(values);
position.needsUpdate = true;
geometry.setDrawRange(0, values.length / 3);
}
function clearLaserScan() {
if (!laserGroup) return;
laserGroup.visible = false;
laserFillMesh.visible = false;
laserLineSegments.visible = false;
laserOpenSpaceLineSegments.visible = false;
laserFrontBeam.visible = false;
}
function setNavigationSensorVisible(visible) {
navigationSensorVisible = visible;
sensorToggleButton.classList.toggle('active', visible);
sensorToggleButton.setAttribute('aria-pressed', String(visible));
if (sensorPointCloudLayer) sensorPointCloudLayer.visible = visible;
if (!visible) {
if (sensorPointCloudRefreshTimer) {
window.clearInterval(sensorPointCloudRefreshTimer);
sensorPointCloudRefreshTimer = null;
}
lastLaserRenderAtMs = 0;
clearLaserScan();
clearSensorPointCloud();
drawMiniMap(latestSlamState);
return;
}
if (SHOW_SENSOR_POINT_CLOUD) {
void refreshSensorPointCloud();
if (!sensorPointCloudRefreshTimer) {
sensorPointCloudRefreshTimer = window.setInterval(() => void refreshSensorPointCloud(), 750);
}
}
if (latestScanMetadata && latestScanRanges) {
drawLaserScan({ ...latestScanMetadata, ranges: latestScanRanges });
}
drawMiniMap(latestSlamState);
}
function drawLaserScan(scan, scanPose = (
NAV2_VIEW_MODE === 'navigation'
? (navigationSceneUsesMapFrame() ? latestSlamState?.scan_pose_map : latestSlamState?.scan_pose)
: latestSlamState?.scan_pose_map
) || null) {
// Live laser data is controlled exclusively by the sensor toggle. Do not
// let a state or binary websocket update redraw it while the toggle is off.
if (!navigationSensorVisible || (NAV2_VIEW_MODE === 'navigation' && !(navigationInitialPoseSet || navigationLocalizationReady))) {
clearLaserScan();
return;
}
if (!scan?.ranges?.length) return;
// The state socket and scan binary socket can deliver the same scan nearly
// together. Rebuilding transparent scan geometry twice stalls the viewer.
const now = performance.now();
if (now - lastLaserRenderAtMs < LASER_RENDER_INTERVAL_MS) return;
lastLaserRenderAtMs = now;
clearLaserScan();
const sourcePose = scanPose || currentRobotPose;
const renderPose = USE_MKMINI_MODEL && scanPose
? {
...sourcePose,
x: sourcePose.x - Math.cos(sourcePose.yaw || 0) * MKMINI_LASER_RENDER_BACKWARD_OFFSET,
y: sourcePose.y - Math.sin(sourcePose.yaw || 0) * MKMINI_LASER_RENDER_BACKWARD_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 = [];
const openSpaceLinePoints = [];
const showOpenSpaceRays = NAV2_VIEW_MODE === 'slam';
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(renderPose, 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 });
if (valid) {
linePoints.push(start.x, start.y, start.z, point.x, point.y, point.z);
} else if (showOpenSpaceRays && i % LASER_OPEN_SPACE_RAY_STRIDE === 0) {
// Match Gazebo's ray visualization: no-return rays remain visible, but
// deliberately subdued so detected obstacles stay the primary signal.
openSpaceLinePoints.push(start.x, start.y, start.z, point.x, point.y, point.z);
}
}
const fillPoints = [];
for (let i = 0; i < endpoints.length; i++) {
const curr = endpoints[i];
const next = endpoints[(i + 1) % endpoints.length];
if (!curr.hit || !next.hit) continue;
fillPoints.push(curr.start.x, curr.start.y, curr.start.z);
fillPoints.push(curr.point.x, curr.point.y, curr.point.z);
fillPoints.push(next.point.x, next.point.y, next.point.z);
fillPoints.push(curr.start.x, curr.start.y, curr.start.z);
fillPoints.push(next.point.x, next.point.y, next.point.z);
fillPoints.push(next.start.x, next.start.y, next.start.z);
}
ensureLaserScanLayer();
updateLaserGeometry(laserFillMesh.geometry, fillPoints);
laserFillMesh.visible = fillPoints.length > 0;
updateLaserGeometry(laserLineSegments.geometry, linePoints);
laserLineSegments.visible = linePoints.length > 0;
updateLaserGeometry(laserOpenSpaceLineSegments.geometry, openSpaceLinePoints);
laserOpenSpaceLineSegments.visible = openSpaceLinePoints.length > 0;
const frontIndex = Math.max(0, Math.min(scan.ranges.length - 1, Math.round((0 - angleMin) / angleInc)));
const front = endpoints[frontIndex];
// Keep the forward direction beam even in open space. It is the visual
// heading reference used in the TurtleBot SLAM view, not only a hit marker.
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) {
laserFrontBeam.position.set(
(front.start.x + front.point.x) / 2,
(front.start.y + front.point.y) / 2,
(front.start.z + front.point.z) / 2
);
laserFrontBeam.scale.set(1, len, 1);
laserFrontBeam.quaternion.setFromUnitVectors(new THREE.Vector3(0, 1, 0), delta.normalize());
laserFrontBeam.visible = true;
} else {
laserFrontBeam.visible = false;
}
} else {
laserFrontBeam.visible = false;
}
}
function updatePointGeometry(geometry, points) {
if (!geometry || !Array.isArray(points)) return;
const positions = new Float32Array(points.length * 3);
let count = 0;
for (const point of points) {
if (!Array.isArray(point) || point.length < 3) continue;
const x = Number(point[0]);
const y = Number(point[1]);
const z = Number(point[2]);
if (!Number.isFinite(x) || !Number.isFinite(y) || !Number.isFinite(z)) continue;
positions[count++] = x;
positions[count++] = y;
positions[count++] = z;
}
if (count === 0) {
geometry.deleteAttribute('position');
geometry.boundingBox = null;
geometry.boundingSphere = null;
return;
}
geometry.setAttribute('position', new THREE.BufferAttribute(positions.slice(0, count), 3));
geometry.computeBoundingSphere();
}
function updatePointCloud(payload) {
if (!pointCloudLayer) return;
const frameId = String(payload?.frame_id || '').replace(/^\/+/, '');
// RTAB-Map's /cloud_map is a complete, transient map in `map`. The raw
// LiDAR fallback is useful before that arrives, but must never overwrite a
// loaded navigation map after localization starts.
if (frameId !== 'map' && hasGlobalMapPointCloud) return;
updatePointGeometry(pointCloudGeometry, payload?.points);
if (frameId === 'map') {
hasGlobalMapPointCloud = true;
// The saved 3D cloud is now available. The occupancy grid was only a
// startup fallback while the exported cloud was loading.
if (navigationMapMesh && SHOW_POINT_CLOUD) {
navigationMapRoot.remove(navigationMapMesh);
navigationMapMesh.geometry.dispose();
navigationMapMesh.material.dispose();
navigationMapMesh = null;
navigationMapTexture?.dispose();
navigationMapTexture = null;
}
}
const transform = payload?.transform;
if (!transform || !Number.isFinite(Number(transform.x)) || !Number.isFinite(Number(transform.y))) {
// A cloud already authored in `map` does not need a transform. Hide
// clouds in an unresolved local frame instead of drawing them under or
// away from the saved navigation map.
pointCloudLayer.visible = frameId === 'map';
pointCloudLayer.position.set(0, 0, 0);
pointCloudLayer.rotation.set(0, 0, 0);
return;
}
pointCloudLayer.position.set(Number(transform.x), Number(transform.y), Number(transform.z) || 0);
pointCloudLayer.rotation.set(0, 0, Number(transform.yaw) || 0);
pointCloudLayer.visible = true;
}
function updateSensorPointCloud(payload) {
if (!sensorPointCloudLayer) return;
const receivedAt = Number(payload?.received_at);
if (!Number.isFinite(receivedAt) || Date.now() - receivedAt * 1000 > SENSOR_POINT_CLOUD_MAX_AGE_MS) {
clearSensorPointCloud();
return;
}
updatePointGeometry(sensorPointCloudGeometry, payload?.points);
const transform = payload?.transform;
if (!transform || !Number.isFinite(Number(transform.x)) || !Number.isFinite(Number(transform.y))) {
sensorPointCloudLayer.visible = false;
return;
}
sensorPointCloudLayer.position.set(Number(transform.x), Number(transform.y), Number(transform.z) || 0);
sensorPointCloudLayer.rotation.set(0, 0, Number(transform.yaw) || 0);
sensorPointCloudLayer.visible = navigationSensorVisible;
}
function clearSensorPointCloud() {
if (!sensorPointCloudLayer || !sensorPointCloudGeometry) return;
updatePointGeometry(sensorPointCloudGeometry, []);
sensorPointCloudLayer.visible = false;
}
async function refreshPointCloud() {
if (!SHOW_POINT_CLOUD || savedMapPointCloudLoaded || pointCloudRequestInFlight) return;
pointCloudRequestInFlight = true;
try {
const response = await fetch(`${BACKEND_URL}/point-cloud`);
if (!response.ok) return;
const payload = await response.json();
updatePointCloud(payload);
// A navigation map exported from the selected DB is immutable during this
// session. Keep polling only for live 3D SLAM clouds.
if (payload?.saved_map === true) {
savedMapPointCloudLoaded = true;
if (pointCloudRefreshTimer) {
window.clearInterval(pointCloudRefreshTimer);
pointCloudRefreshTimer = null;
}
}
} catch (_) {
// The 2D SLAM view remains usable while the 3D point-cloud topic starts.
} finally {
pointCloudRequestInFlight = false;
}
}
async function refreshSensorPointCloud() {
if (!SHOW_SENSOR_POINT_CLOUD || !navigationSensorVisible || sensorPointCloudRequestInFlight) return;
sensorPointCloudRequestInFlight = true;
try {
const response = await fetch(`${BACKEND_URL}/sensor-point-cloud`);
if (!response.ok) {
clearSensorPointCloud();
return;
}
updateSensorPointCloud(await response.json());
} catch (_) {
// The static 3D map remains usable while the live lidar stream starts.
} finally {
sensorPointCloudRequestInFlight = false;
}
}
function setCameraPreviewAvailable(available) {
cameraPreviewWrap?.classList.toggle('is-visible', available);
if (available) return;
if (cameraFrameObjectUrl) {
URL.revokeObjectURL(cameraFrameObjectUrl);
cameraFrameObjectUrl = null;
}
if (cameraPreview) cameraPreview.removeAttribute('src');
}
async function refreshCameraFrame() {
if (!ENABLE_CAMERA_PREVIEW || !REAL_RUNTIME || cameraFrameRequestInFlight) return;
cameraFrameRequestInFlight = true;
try {
const response = await fetch(`${BACKEND_URL}/camera-frame`, { cache: 'no-store' });
if (!response.ok) {
setCameraPreviewAvailable(false);
return;
}
const nextUrl = URL.createObjectURL(await response.blob());
const previousUrl = cameraFrameObjectUrl;
cameraFrameObjectUrl = nextUrl;
if (cameraPreview) cameraPreview.src = nextUrl;
if (previousUrl) URL.revokeObjectURL(previousUrl);
setCameraPreviewAvailable(true);
} catch (_) {
setCameraPreviewAvailable(false);
} finally {
cameraFrameRequestInFlight = false;
}
}
function drawMiniMap(state) {
pendingMiniMapState = state;
if (miniMapRenderTimer) return;
const elapsed = performance.now() - lastMiniMapRenderAtMs;
const wait = Math.max(0, MINI_MAP_RENDER_INTERVAL_MS - elapsed);
miniMapRenderTimer = window.setTimeout(() => {
miniMapRenderTimer = null;
lastMiniMapRenderAtMs = performance.now();
drawMiniMapNow(pendingMiniMapState);
}, wait);
}
function drawMiniMapNow(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 ((NAV2_VIEW_MODE !== 'navigation' || navigationSensorVisible) && 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 (shouldRenderRobot() && (state?.pose || state?.pose_map)) drawLink(odomFrame, robotFrame);
drawFrame(mapFrame, 'map');
drawFrame(odomFrame, 'odom');
if (shouldRenderRobot() && (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;
// MAP1 and its metadata are sent as separate WebSocket frames. Accept the
// first metadata frame even when its binary payload arrived first; only a
// fully rendered saved navigation map is locked against later local maps.
if (navigationMapLocked && navigationMapMesh) return;
latestMiniMapGrid = {
...(latestMiniMapGrid || {}),
...map,
data: latestMiniMapGrid?.data || null,
};
// Map metadata and its binary occupancy payload arrive on separate socket
// frames. When the payload wins the race, retry now that origin/resolution
// are available so the floor map is not left empty.
if (latestMiniMapGrid.data?.length) renderNavigationOccupancyMap();
}
function updateNavigationMapTransform() {
if (NAV2_VIEW_MODE !== 'navigation') return;
const transform = navigationMapToOdom || { x:0, y:0, yaw:0 };
if (navigationSceneUsesMapFrame()) {
navigationMapRoot.position.set(0, 0, 0);
navigationMapRoot.rotation.set(0, 0, 0);
// The saved map and the Gazebo world share the coordinates captured
// during SLAM. AMCL's live map -> odom correction applies to the robot
// estimate and must not move the static world geometry.
gazeboWorldRoot.position.set(0, 0, 0);
gazeboWorldRoot.rotation.set(0, 0, 0);
return;
}
gazeboWorldRoot.position.set(0, 0, 0);
gazeboWorldRoot.rotation.set(0, 0, 0);
const yaw = transform.yaw || 0;
const c = Math.cos(yaw);
const s = Math.sin(yaw);
// `map_to_odom` is map <- odom. The map mesh is authored in map
// coordinates, so the odom scene needs its inverse transform.
navigationMapRoot.position.set(
-c * (transform.x || 0) - s * (transform.y || 0),
s * (transform.x || 0) - c * (transform.y || 0),
0,
);
navigationMapRoot.rotation.set(0, 0, -yaw);
}
function renderNavigationOccupancyMap() {
const grid = latestMiniMapGrid;
if (NAV2_VIEW_MODE !== 'navigation' || !grid?.data?.length || !grid.width || !grid.height || !grid.resolution || !grid.origin) return false;
// The occupancy grid is a short-lived fallback for a saved 3D map. It
// keeps the scene usable if the point-cloud export is still starting.
if (SHOW_POINT_CLOUD && hasGlobalMapPointCloud) return false;
const canvas = document.createElement('canvas');
canvas.width = grid.width;
canvas.height = grid.height;
const context = canvas.getContext('2d');
const image = context.createImageData(grid.width, grid.height);
for (let y = 0; y < grid.height; y++) {
for (let x = 0; x < grid.width; x++) {
const source = (grid.height - 1 - y) * grid.width + x;
const target = (y * grid.width + x) * 4;
const value = grid.data[source];
const color = value < 0 ? 102 : value >= 50 ? 24 : 226;
image.data[target] = color;
image.data[target + 1] = color;
image.data[target + 2] = color;
image.data[target + 3] = value < 0 ? 170 : 235;
}
}
context.putImageData(image, 0, 0);
if (navigationMapMesh) {
navigationMapRoot.remove(navigationMapMesh);
navigationMapMesh.geometry.dispose();
navigationMapMesh.material.dispose();
}
navigationMapTexture?.dispose();
navigationMapTexture = new THREE.CanvasTexture(canvas);
navigationMapTexture.colorSpace = THREE.SRGBColorSpace;
navigationMapTexture.magFilter = THREE.NearestFilter;
navigationMapTexture.minFilter = THREE.NearestFilter;
navigationMapTexture.flipY = true;
const width = grid.width * grid.resolution;
const height = grid.height * grid.resolution;
navigationMapMesh = new THREE.Mesh(
new THREE.PlaneGeometry(width, height),
new THREE.MeshBasicMaterial({ map:navigationMapTexture, transparent:true, opacity:0.58, side:THREE.DoubleSide, depthWrite:false }),
);
const originYaw = Number(grid.origin.yaw || 0);
const centerX = grid.origin.x + Math.cos(originYaw) * width / 2 - Math.sin(originYaw) * height / 2;
const centerY = grid.origin.y + Math.sin(originYaw) * width / 2 + Math.cos(originYaw) * height / 2;
navigationMapMesh.position.set(centerX, centerY, 0.012);
navigationMapMesh.rotation.z = originYaw;
navigationMapRoot.add(navigationMapMesh);
updateNavigationMapTransform();
// A saved 3D map is the navigation reference. RTAB-Map can publish a small
// local occupancy update later on /map; replacing the initial complete map
// with it makes the floor map appear to shrink during localization.
if (LOCK_NAVIGATION_MAP) navigationMapLocked = true;
if (!navigationMapFocused) {
const sceneCenter = navigationSceneUsesMapFrame()
? { x:centerX, y:centerY }
: mapToOdom({ x:centerX, y:centerY, yaw:originYaw });
const center = sdfPointToWorld(sceneCenter.x, sceneCenter.y, 0);
const distance = Math.max(width, height) * 1.1;
controls.target.copy(center);
camera.position.set(center.x + distance * .62, center.y + distance * .86, center.z + distance * .62);
camera.lookAt(center);
navigationMapFocused = true;
}
return true;
}
function applyMapBinary(buffer) {
if (navigationMapLocked) return;
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;
// Do not lock a selected navigation map before its separate metadata frame
// supplies resolution and origin. Otherwise Three.js receives valid /map
// data but cannot construct a plane from it.
renderNavigationOccupancyMap();
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.196',
'',
].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;
setMapSavePending(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 {
setMapSavePending(false);
downloadMapButton.disabled = !latestMiniMapGrid?.data?.length;
focusViewer();
}
});
pathToggleButton.addEventListener('click', () => {
setPathVisible(!pathVisible);
focusViewer();
});
sensorToggleButton.addEventListener('click', () => {
setNavigationSensorVisible(!navigationSensorVisible);
focusViewer();
});
cameraPreviewToggle?.addEventListener('click', () => {
const collapsed = cameraPreviewWrap?.classList.toggle('is-collapsed') ?? false;
cameraPreviewToggle.setAttribute('aria-expanded', String(!collapsed));
cameraPreviewToggle.setAttribute('aria-label', collapsed ? '카메라 화면 펼치기' : '카메라 화면 접기');
});
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', () => {
if (poseToggleButton.disabled) return;
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();
});
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' && !navigationSceneUsesMapFrame()
? 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 scenePose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? mapToOdom(pose) : pose;
navigationGoalPreview.position.set(scenePose.x, scenePose.y, 0.06);
navigationGoalPreview.rotation.z = scenePose.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: 1.0,
};
}
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 scenePose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? mapToOdom(pose) : pose;
obstaclePreview.position.set(scenePose.x, scenePose.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 scenePose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? mapToOdom(pose) : pose;
return new THREE.Vector3(scenePose.x, scenePose.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' && !navigationSceneUsesMapFrame()
? odomToMap({ x: sdfPoint.x, y: sdfPoint.y, yaw: currentRobotPose.yaw || 0 })
: sdfPoint;
const robotMapPose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame()
? odomToMap(currentRobotPose)
: currentRobotPose;
const headingToGoal = Math.atan2(
mapPoint.y - (robotMapPose?.y || 0),
mapPoint.x - (robotMapPose?.x || 0),
);
return {
x: mapPoint.x,
y: mapPoint.y,
// A Dubins-based Nav2 planner must know which way the vehicle should face
// at the destination. The clicked ground point has no yaw, so using zero
// forced every goal to face east and made otherwise valid paths fail.
yaw: Number.isFinite(headingToGoal) ? headingToGoal : (mapPoint.yaw || 0),
};
}
function showInitialPosePreview(pose) {
if (!posePreviewModel) return;
const scenePose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? 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;
}
function moveLocalSimulatorToInitialPose(pose) {
const requestId = `sim-pose-${Date.now()}-${Math.random().toString(36).slice(2, 8)}`;
return new Promise((resolve, reject) => {
const timeout = window.setTimeout(() => {
pendingSimulatorPoseRequests.delete(requestId);
reject(new Error('가제보 로봇 위치 적용 시간이 초과되었습니다.'));
}, 12_000);
pendingSimulatorPoseRequests.set(requestId, { resolve, reject, timeout });
window.parent.postMessage({
source: 'autonomy-slam-viewer',
type: 'simulator-pose-request',
requestId,
pose: { x: pose.x, y: pose.y, yaw: pose.yaw || 0 },
}, '*');
});
}
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');
const result = await response.json().catch(() => ({}));
// RTAB-Map first rebases ICP odometry to this nearby seed, then refines it
// against the saved map. Give its first corrected TF a short head start
// before enabling a navigation goal.
if (result.refining) await new Promise((resolve) => window.setTimeout(resolve, 750));
navigationInitialPoseSet = true;
// A real-platform click is only a localization seed. Keep the model on
// the live TF/odometry pose instead of drawing it at the clicked yaw.
syncGoalToggleAvailability();
navigationPlanSuppressed = false;
if (robotModel) robotModel.visible = true;
hideInitialPosePreview();
return;
}
// In simulation, the selected point is the robot's actual Gazebo spawn
// point as well as AMCL's initial map pose. Moving only AMCL left the lidar
// at the old spawn location and rotated the scan against the saved map.
await moveLocalSimulatorToInitialPose(pose);
await new Promise((resolve) => window.setTimeout(resolve, 200));
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;
applyInitialNavigationPose(pose);
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 scenePoint = localGazeboNavigationUsesOdom() ? mapToOdom(goal) : goal;
const obstacle = {
id: `runtime-wall-${Date.now()}-${Math.random().toString(36).slice(2, 7)}`,
x: scenePoint.x,
y: scenePoint.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;
resetNavigationPoseForNewSession(state);
restoreNavigationPoseFromState(state);
updateNavigationLocalizationState(state);
updateRosTopicStatus(state?.topic_status);
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 || state?.sensor_point_cloud) markVisualizationResponseReceived();
if (NAV2_VIEW_MODE === 'navigation' && state?.tf?.map_to_odom) {
navigationMapToOdom = state.tf.map_to_odom;
updateNavigationMapTransform();
}
updateNavigationPlan(state?.plan);
// Remote navigation is rendered in the map frame, matching the saved map,
// room model, AMCL pose, route and goal. Local Gazebo remains odom-based.
const visualizationPose = (REAL_RUNTIME && (SHOW_POINT_CLOUD || (NAV2_VIEW_MODE === 'navigation' && state?.pose_3d)))
? (state?.pose_3d || state?.pose)
: (NAV2_VIEW_MODE === 'navigation'
? (navigationSceneUsesMapFrame() ? (state?.pose_map || state?.pose) : state?.pose)
: (state?.pose_map || state?.pose));
if (visualizationPose) {
setRobotPose({
...visualizationPose,
steeringAngle: state?.vehicle?.steering_angle,
steeringLeftAngle: state?.vehicle?.steering_left_angle,
steeringRightAngle: state?.vehicle?.steering_right_angle,
motion: state?.pose ?? 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();
}
async function loadInitialSlamState() {
try {
const response = await fetch(`${BACKEND_URL}/state`);
if (!response.ok) return;
applySlamState(await response.json());
} catch (_) {
// The websocket reconnect path remains the fallback while the bridge starts.
}
}
void loadInitialSlamState();
if (SHOW_POINT_CLOUD) {
void refreshPointCloud();
pointCloudRefreshTimer = window.setInterval(() => void refreshPointCloud(), 1000);
}
if (SHOW_SENSOR_POINT_CLOUD && navigationSensorVisible) {
void refreshSensorPointCloud();
sensorPointCloudRefreshTimer = window.setInterval(() => void refreshSensorPointCloud(), 750);
}
if (ENABLE_CAMERA_PREVIEW && REAL_RUNTIME) {
void refreshCameraFrame();
cameraFrameRefreshTimer = window.setInterval(() => void refreshCameraFrame(), 200);
}
window.addEventListener('beforeunload', () => {
if (pointCloudRefreshTimer) window.clearInterval(pointCloudRefreshTimer);
if (sensorPointCloudRefreshTimer) window.clearInterval(sensorPointCloudRefreshTimer);
if (miniMapRenderTimer) window.clearTimeout(miniMapRenderTimer);
if (cameraFrameRefreshTimer) window.clearInterval(cameraFrameRefreshTimer);
if (cameraFrameObjectUrl) URL.revokeObjectURL(cameraFrameObjectUrl);
});
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;
const MKMINI_GAZEBO_WHEELBASE_METERS = 0.6;
const MKMINI_GAZEBO_MAX_STEERING_RADIANS = 0.5;
let cmdVelRequestInFlight = false;
let queuedCmdVel = null;
let mkMiniTeleopSteeringAngle = 0;
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 (REAL_RUNTIME) return false;
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 setSimulatorMkMiniSteering(angle) {
mkMiniTeleopSteeringAngle = THREE.MathUtils.clamp(
Number(angle) || 0,
-MKMINI_GAZEBO_MAX_STEERING_RADIANS,
MKMINI_GAZEBO_MAX_STEERING_RADIANS,
);
const fallbackToTwistSteering = () => {
if (Math.abs(mkMiniTeleopSteeringAngle) < 0.001) return;
// The Gazebo tricycle plugin consumes angular.z as its steering angle.
sendCmdVel(0, mkMiniTeleopSteeringAngle);
};
fetch(`${BACKEND_URL}/steering`, {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify({ angle: mkMiniTeleopSteeringAngle }),
})
.then((response) => {
if (!response.ok) fallbackToTwistSteering();
})
.catch((error) => {
console.warn('[teleop] steering update failed', error);
fallbackToTwistSteering();
});
}
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;
if (USE_MKMINI_MODEL && !REAL_RUNTIME) {
if (activeMoveKey === 'j' || activeMoveKey === 'l') {
setSimulatorMkMiniSteering(angularSign * MKMINI_GAZEBO_MAX_STEERING_RADIANS);
return;
}
if (linearSign !== 0 && Math.abs(mkMiniTeleopSteeringAngle) > 0.001) {
const linear = linearSign * maxLinear;
// Gazebo's tricycle drive consumes angular.z as the front steering
// angle. Keep this identical to the independently rendered front tyres.
sendCmdVel(linear, mkMiniTeleopSteeringAngle);
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;
if (USE_MKMINI_MODEL && !REAL_RUNTIME) setSimulatorMkMiniSteering(0);
else mkMiniTeleopSteeringAngle = 0;
stopRobot();
return;
}
if (USE_MKMINI_MODEL && !REAL_RUNTIME && (key === 'j' || key === 'l')) {
activeMoveKey = key;
sendActiveMoveCommand();
activeMoveKey = null;
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 || SAVED_GAZEBO_MAP_WORLD || REAL_RUNTIME
? []
: [buildFromSdf().catch((error) => console.warn('world model render failed', error))];
const remoteNavigationSceneTasks = [];
Promise.all(EDITOR_MODE
? worldSceneTasks
: [...worldSceneTasks, ...remoteNavigationSceneTasks, (USE_MKMINI_MODEL ? loadMkMiniModel : loadBurgerModel)({ ...DEFAULT_SIMULATION_SPAWN, 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();
updateMkMiniFrontWheelSteering();
controls.update();
renderer.render(scene, camera);
}
animate();
</script>
</body>
</html>