Files
lms-fe/src/renderer/public/autoware-viewer/index.html
T
2026-06-24 11:22:52 +09:00

3346 lines
137 KiB
HTML

<!DOCTYPE html>
<html lang="ja">
<head>
<meta charset="UTF-8" />
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
<title>Autoware Map Viewer</title>
<style>
*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
body { background: #0f0f1a; color: #e0e0f0; font-family: 'Courier New', monospace; overflow: hidden; }
#app { width: 100vw; height: 100vh; display: block; }
/* ── Loading ── */
#loading {
position: fixed; inset: 0; z-index: 100;
display: flex;
align-items: center; justify-content: center;
background: #0f0f1a;
}
#loading-spinner {
width: 26px; height: 26px;
border: 2px solid rgba(255,255,255,0.18);
border-top-color: rgba(255,255,255,0.92);
border-radius: 50%;
animation: loading-spin 0.8s linear infinite;
}
@keyframes loading-spin {
to { transform: rotate(360deg); }
}
/* ── Layer panel ── */
#panel-wrap { position: fixed; top: 12px; left: 12px; z-index: 10; display: none; }
#panel-toggle {
width: 38px; height: 38px;
background: rgba(10,10,30,0.88); border: 1px solid #334; border-radius: 8px;
display: flex; align-items: center; justify-content: center;
cursor: pointer; backdrop-filter: blur(6px);
font-size: 17px; color: #7fbbff; user-select: none; transition: background 0.15s;
}
#panel-toggle:hover { background: rgba(30,30,60,0.95); }
#panel {
background: rgba(10,10,30,0.88); border: 1px solid #334;
border-radius: 10px; padding: 14px 16px; min-width: 210px;
backdrop-filter: blur(6px); margin-top: 6px; display: none;
}
#panel h3 { font-size: 13px; color: #aaccff; letter-spacing: 1px; margin-bottom: 12px; }
.layer-row { display: flex; align-items: center; gap: 8px; margin-bottom: 7px; cursor: pointer; user-select: none; }
.layer-row:hover .layer-label { color: #fff; }
.layer-check {
width: 14px; height: 14px; border: 1px solid #557; border-radius: 3px;
display: flex; align-items: center; justify-content: center; flex-shrink: 0; transition: background 0.15s;
}
.layer-check.on { background: #4488ff; border-color: #4488ff; }
.layer-check.on::after { content: '✓'; font-size: 10px; color: #fff; line-height: 1; }
.layer-dot { width: 8px; height: 8px; border-radius: 50%; flex-shrink: 0; }
.layer-label { font-size: 12px; color: #aab; }
.divider { height: 1px; background: #334; margin: 10px 0; }
.stat-row { font-size: 11px; color: #668; display: flex; justify-content: space-between; margin-bottom: 4px; }
.stat-val { color: #99bbdd; }
.hint { font-size: 10px; color: #445; margin-top: 10px; line-height: 1.8; }
/* ── Pose toolbar (bottom center) ── */
#pose-toolbar {
position: fixed; bottom: 24px; left: 50%; transform: translateX(-50%);
z-index: 10; display: none;
background: rgba(10,10,30,0.90); border: 1px solid #334; border-radius: 12px;
padding: 8px 12px; gap: 8px; align-items: center;
backdrop-filter: blur(8px);
}
.pose-btn {
padding: 7px 16px; border: 1px solid transparent; border-radius: 7px;
font-family: 'Courier New', monospace; font-size: 12px; cursor: pointer;
transition: all 0.15s; display: flex; align-items: center; gap: 6px;
user-select: none;
}
.pose-btn.initial {
background: rgba(0,80,160,0.6); color: #88ccff; border-color: #336699;
}
.pose-btn.initial:hover { background: rgba(0,100,200,0.8); color: #fff; border-color: #0088ff; }
.pose-btn.initial.active {
background: #0066cc; color: #fff; border-color: #00aaff;
box-shadow: 0 0 12px rgba(0,136,255,0.5);
}
.pose-btn.goal {
background: rgba(120,50,0,0.6); color: #ffaa44; border-color: #664422;
}
.pose-btn.goal:hover { background: rgba(160,70,0,0.8); color: #fff; border-color: #ff6600; }
.pose-btn.goal.active {
background: #cc5500; color: #fff; border-color: #ff7700;
box-shadow: 0 0 12px rgba(255,100,0,0.5);
}
.pose-btn.clear { background: rgba(20,20,40,0.8); color: #556; border-color: #334; }
.pose-btn.clear:hover { background: rgba(40,20,20,0.9); color: #ff8888; border-color: #553333; }
.pose-btn.drive { background: rgba(0,120,60,0.6); color: #88ffbb; border-color: #336644; }
.pose-btn.drive:hover { background: rgba(0,160,80,0.85); color: #fff; border-color: #00cc66; }
.pose-btn.drive:disabled { opacity: 0.35; cursor: not-allowed; }
.toolbar-sep { width: 1px; height: 22px; background: #334; flex-shrink: 0; }
/* plan status */
#plan-status {
position: fixed; bottom: 100px; left: 50%; transform: translateX(-50%);
font-size: 11px; color: #88ccaa; background: rgba(10,10,30,0.8);
padding: 4px 12px; border-radius: 5px; pointer-events: none;
display: none; white-space: nowrap;
}
.pose-hint-text {
font-size: 10px; color: #446; padding: 0 4px; white-space: nowrap;
display: none;
}
.pose-hint-text.visible { display: block; }
/* ── Pose info panel (right side) ── */
#pose-info-wrap {
position: fixed; right: 12px; top: 12px; z-index: 10; display: none;
}
#pose-info-toggle {
width: 38px; height: 38px; margin-left: auto;
background: rgba(10,10,30,0.88); border: 1px solid #334; border-radius: 8px;
display: flex; align-items: center; justify-content: center;
cursor: pointer; backdrop-filter: blur(6px);
font-size: 15px; color: #445; user-select: none; transition: background 0.15s;
}
#pose-info-toggle:hover { background: rgba(30,30,60,0.95); }
#pose-info {
background: rgba(10,10,30,0.88); border: 1px solid #334; border-radius: 10px;
padding: 13px 15px; backdrop-filter: blur(6px); min-width: 195px;
margin-top: 6px; display: none;
}
.pi-section { margin-bottom: 10px; }
.pi-title { font-size: 10px; letter-spacing: 1px; margin-bottom: 7px; font-weight: bold; }
.pi-title.initial { color: #66aaff; }
.pi-title.goal { color: #ff8833; }
.pi-row { display: flex; justify-content: space-between; margin-bottom: 3px; font-size: 11px; color: #556; }
.pi-row .pi-val { color: #aaccdd; }
.pi-row .pi-val.heading { color: #88ffcc; }
.pi-divider { height: 1px; background: #334; margin: 8px 0; }
/* ── Colorbar ── */
#colorbar-wrap {
position: fixed; bottom: 20px; right: 16px; z-index: 10; display: none;
}
#colorbar-toggle {
width: 38px; height: 38px; margin-left: auto;
background: rgba(10,10,30,0.85); border: 1px solid #334; border-radius: 8px;
display: flex; align-items: center; justify-content: center;
cursor: pointer; font-size: 14px; color: #778; user-select: none;
transition: background 0.15s;
}
#colorbar-toggle:hover { background: rgba(30,30,60,0.95); }
#colorbar {
background: rgba(10,10,30,0.85); border: 1px solid #334; border-radius: 8px;
padding: 10px 12px; margin-top: 6px; display: none;
}
#colorbar-title { font-size: 11px; color: #aab; margin-bottom: 6px; text-align: center; }
#colorbar-grad {
width: 20px; height: 100px; border-radius: 3px;
background: linear-gradient(to top, #0000ff, #00ffff, #00ff00, #ffff00, #ff0000); margin: 0 auto;
}
#colorbar-labels {
display: flex; flex-direction: column; justify-content: space-between;
font-size: 10px; color: #778; height: 100px; margin-top: -100px; padding-left: 28px;
}
/* ── Crosshair cursor in pose mode ── */
body.mode-pose canvas { cursor: crosshair !important; }
/* ── Vehicle HUD ── */
#vehicle-hud {
position: fixed; left: 12px; top: 62px; z-index: 10; display: none;
background: transparent; border: none; padding: 0; user-select: none;
flex-direction: column; gap: 4px; width: 256px;
transition: left 0.2s ease;
}
#vehicle-hud.visible { display: flex; }
#hud-top-row { display: flex; gap: 8px; align-items: stretch; }
#hud-wheel-box {
background: rgba(10,10,30,0.85); border: 1px solid #334; border-radius: 10px;
padding: 8px; backdrop-filter: blur(6px);
display: flex; flex-direction: column; align-items: center; justify-content: center; gap: 4px;
}
#hud-steer-label { text-align: center; font-size: 20px; color: #88ffcc; font-family: 'Courier New', monospace; line-height: 1; }
#hud-steer-unit { font-size: 10px; color: #88ffcc; margin-left: 2px; }
#hud-steering { display: block; }
#hud-speed-box {
flex: 1; min-width: 116px; background: rgba(10,10,30,0.85); border: 1px solid #334; border-radius: 10px;
padding: 8px 6px; backdrop-filter: blur(6px);
display: flex; flex-direction: column; align-items: center; justify-content: center; gap: 4px; overflow: hidden;
}
#hud-speed-gauge-canvas { display: block; }
#hud-speed-row { text-align: center; font-size: 20px; color: #88ffcc; font-family: 'Courier New', monospace; line-height: 1; }
#hud-speed-unit { font-size: 10px; color: #88ffcc; margin-left: 2px; }
#hud-speed-limit { text-align: center; font-family: 'Courier New', monospace; line-height: 1.3; font-size: 10px; color: #fff; }
#hud-speed-limit span { font-size: 13px; color: #ff8844; font-weight: bold; }
#hud-signals-box {
background: rgba(10,10,30,0.85); border: 1px solid #334; border-radius: 8px;
padding: 4px 12px; backdrop-filter: blur(6px);
display: flex; justify-content: space-between; align-items: center;
}
.hud-signal { font-size: 22px; color: rgba(200,200,200,0.55); transition: color 0.08s, text-shadow 0.08s; line-height: 1; }
.hud-signal.active { color: #ffaa00; text-shadow: 0 0 8px rgba(255,160,0,0.8); }
@media (max-width: 700px), (max-height: 500px) {
.pose-info-toggle{display:none}
.pose-info-wrap{display:none}
.pose-label{display:none}
.pose-btn{width:36px;height:36px;padding:0;justify-content:center}
.toolbar-sep{display:none}
#panel-wrap { display: none !important; }
#vehicle-hud { display: none !important; }
#pose-toolbar { transform: translateX(-50%) scale(0.82); bottom: 12px; }
#panel-wrap { top: 8px; left: 8px; }
#colorbar-wrap { display: none !important; }
#plan-status{bottom:60px}
}
</style>
</head>
<body>
<canvas id="app"></canvas>
<!-- Loading -->
<div id="loading">
<div id="loading-spinner"></div>
</div>
<!-- Layer panel -->
<div id="panel-wrap">
<div style="display:flex; gap:6px;">
<div id="panel-toggle" onclick="togglePanel()" title="Map Layers">&#9741;</div>
</div>
<div id="panel">
<h3>&#9654; MAP LAYERS</h3>
<div class="layer-row" onclick="toggleLayer('road')">
<div class="layer-check on" id="chk-road"></div>
<div class="layer-dot" style="background:#44ff88"></div>
<span class="layer-label">Road Lanelets</span>
</div>
<div class="layer-row" onclick="toggleLayer('crosswalk')">
<div class="layer-check on" id="chk-crosswalk"></div>
<div class="layer-dot" style="background:#ffdd44"></div>
<span class="layer-label">Crosswalks</span>
</div>
<div class="layer-row" onclick="toggleLayer('stopline')">
<div class="layer-check on" id="chk-stopline"></div>
<div class="layer-dot" style="background:#ff4444"></div>
<span class="layer-label">Stop Lines</span>
</div>
<div class="layer-row" onclick="toggleLayer('parking')">
<div class="layer-check on" id="chk-parking"></div>
<div class="layer-dot" style="background:#ff6600"></div>
<span class="layer-label">Parking Areas</span>
</div>
<div class="layer-row" onclick="togglePolarGrid()">
<div class="layer-check on" id="chk-polargrid"></div>
<div class="layer-dot" style="background:#ffffff"></div>
<span class="layer-label">Polar Grid</span>
</div>
<div class="divider"></div>
<div class="stat-row"><span>Road Lanelets</span><span class="stat-val" id="s-road">-</span></div>
<div class="stat-row"><span>Crosswalks</span><span class="stat-val" id="s-cross">-</span></div>
<div class="stat-row"><span>Parking Areas</span><span class="stat-val" id="s-parking">-</span></div>
<div class="divider"></div>
<div class="hint">
🖱 Left drag — Rotate<br>
🖱 Right drag — Pan<br>
🖱 Scroll — Zoom<br>
⌨ F — Reset view &nbsp; T — Top view
</div>
</div>
</div>
<!-- Pose toolbar -->
<div id="pose-toolbar">
<button class="pose-btn initial" id="btn-initial-pose" onclick="setPoseMode('initial')">
<span class="pose-icon">✛</span>
<span class="pose-label">2D Pose Estimate</span>
</button>
<button class="pose-btn goal" id="btn-goal-pose" onclick="setPoseMode('goal')">
<span class="pose-icon">⚑</span>
<span class="pose-label">2D Goal Pose</span>
</button>
<div class="toolbar-sep"></div>
<button class="pose-btn drive" id="btn-drive" onclick="startDrive()" disabled>
<span class="pose-icon">&#9654;</span>
<span class="pose-label">Start</span>
</button>
<div class="toolbar-sep"></div>
<button class="pose-btn clear" onclick="clearPoses()">
<span class="pose-icon">✕</span>
<span class="pose-label">Clear</span>
</button>
<div class="toolbar-sep"></div>
</div>
<div id="plan-status"></div>
<!-- Pose info panel -->
<div id="pose-info-wrap">
<div style="display:flex; justify-content:flex-end;">
<div id="pose-info-toggle" onclick="togglePoseInfo()" title="Pose Info">⌖</div>
</div>
<div id="pose-info">
<div class="pi-section" id="pi-initial-sec" style="display:none">
<div class="pi-title initial">▶ INITIAL POSE</div>
<div class="pi-row"><span>X (local)</span><span class="pi-val" id="pi-ix">—</span></div>
<div class="pi-row"><span>Y (local)</span><span class="pi-val" id="pi-iy">—</span></div>
<div class="pi-row"><span>Heading</span><span class="pi-val heading" id="pi-ih">—</span></div>
</div>
<div class="pi-divider" id="pi-mid-div" style="display:none"></div>
<div class="pi-section" id="pi-goal-sec" style="display:none">
<div class="pi-title goal">⚑ GOAL POSE</div>
<div class="pi-row"><span>X (local)</span><span class="pi-val" id="pi-gx">—</span></div>
<div class="pi-row"><span>Y (local)</span><span class="pi-val" id="pi-gy">—</span></div>
<div class="pi-row"><span>Heading</span><span class="pi-val heading" id="pi-gh">—</span></div>
</div>
<div id="pi-dist-sec" style="display:none">
<div class="pi-divider"></div>
<div class="pi-row"><span>Distance</span><span class="pi-val heading" id="pi-dist">—</span></div>
</div>
</div>
</div>
<!-- Vehicle HUD -->
<div id="vehicle-hud">
<div id="hud-top-row">
<div id="hud-wheel-box">
<canvas id="hud-steering" width="108" height="108"></canvas>
<div id="hud-steer-label"><span id="hud-steer-val">0.0</span><span id="hud-steer-unit">deg</span></div>
</div>
<div id="hud-speed-box">
<canvas id="hud-speed-gauge-canvas" width="112" height="80"></canvas>
<div id="hud-speed-row"><span id="hud-speed-val">0.00</span><span id="hud-speed-unit">km/h</span></div>
<div id="hud-speed-limit">limited<br><span id="hud-limit-val">-</span></div>
</div>
</div>
<div id="hud-signals-box">
<div class="hud-signal" id="hud-left-sig">◄</div>
<div class="hud-signal" id="hud-right-sig">►</div>
</div>
</div>
<!-- Colorbar -->
<div id="colorbar-wrap">
<div id="colorbar-toggle" onclick="toggleColorbar()" title="Height Legend">▲</div>
<div id="colorbar">
<div id="colorbar-title">Height (m)</div>
<div style="display:flex; gap:4px; align-items:stretch">
<div id="colorbar-grad"></div>
<div id="colorbar-labels">
<span id="cb-max">?</span><span></span><span></span><span></span>
<span id="cb-min">?</span>
</div>
</div>
</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 { GLTFLoader } from 'three/addons/loaders/GLTFLoader.js';
import { DRACOLoader } from 'three/addons/loaders/DRACOLoader.js';
// ──────────────────────────────────────────────────────
// Constants
// ──────────────────────────────────────────────────────
//const CENTER_X = 3763.558837890625;
//const CENTER_Y = 73675.99609375;
const MAP_CONFIG = {
'1': {
dir: '/autoware-map/map1',
centerX: 3763.558837890625,
centerY: 73675.99609375,
startX: 3743.91,
startY: 73732.52,
},
'2': {
dir: '/autoware-map/map2',
centerX: 89613.4,
centerY: 42345.3,
startX: 89613.4,
startY: 42345.3,
}
};
const _mapId = new URLSearchParams(location.search).get('map') || '1';
const MAP_CFG = MAP_CONFIG[_mapId] || MAP_CONFIG['1'];
const CENTER_X = MAP_CFG.centerX;
const CENTER_Y = MAP_CFG.centerY;
const MAP_DIR = MAP_CFG.dir;
// map_projector_info.yaml
// projector_type: MGRS, mgrs_grid: 54SVE
// 54SVE is UTM zone 54N, 100km grid square base:
// easting = 400000 + local_x
// northing = 3900000 + local_y
const UTM_ZONE = 54;
const MGRS_EASTING_BASE = 400000;
const MGRS_NORTHING_BASE = 3900000;
let pcdZMin = 0, pcdZMax = 0; // PCD z 범위 (loadPointCloud 후 설정)
let laneEleOffset = 0; // OSM ele → PCD z 정렬 오프셋 (loadLanelet2 후 설정)
// ── 백엔드 API ──
const QUERY = new URLSearchParams(location.search);
const MAP_SOURCE = QUERY.get('source') || 'static';
const ROBOT_STACK = QUERY.get('stack') || 'autoware';
const ROS_BRIDGE_WS = (QUERY.get('bridge') || 'ws://localhost:8765').replace(/\/+$/, '');
const ROS_BRIDGE_HTTP = ROS_BRIDGE_WS.replace(/^ws:/, 'http:').replace(/^wss:/, 'https:');
const USE_ROS_MAP = MAP_SOURCE === 'ros';
const USE_PCD_ON_START = QUERY.get('pcd') === '1';
const NAV2_VEHICLE_LENGTH = Number(QUERY.get('vehicle_length')) || 1.0;
const NAV2_VEHICLE_WIDTH = Number(QUERY.get('vehicle_width')) || 0.6;
const NAV2_VEHICLE_HEIGHT = Number(QUERY.get('vehicle_height')) || 0.35;
const NAV2_BASE_TO_CENTER = Number(QUERY.get('base_to_center')) || 0.0;
let rosMapCenter = null;
let rosPointcloudPayload = null;
let rosVectorPayload = null;
let rosVectorRevision = null;
let rosPointcloudAcceptedAfterVector = false;
//const _mapId = new URLSearchParams(location.search).get('map') || '1';
//const MAP_DIR = _mapId === '2' ? '/autoware-map/map2' : '/autoware-map/map1';
// ──────────────────────────────────────────────────────
// Renderer & Scene
// ──────────────────────────────────────────────────────
const canvas = document.getElementById('app');
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
renderer.setSize(window.innerWidth, window.innerHeight);
renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
const scene = new THREE.Scene();
scene.background = new THREE.Color(0x0f0f1a);
const camera = new THREE.PerspectiveCamera(55, window.innerWidth / window.innerHeight, 0.5, 3000);
camera.layers.enable(1); // 차량 메시(레이어 1) 포함
const START_X = MAP_CFG.startX - CENTER_X;
const START_Z = -(MAP_CFG.startY - CENTER_Y);
camera.position.set(START_X, 80, START_Z + 0.01);
camera.lookAt(START_X, 0, START_Z);
const controls = new OrbitControls(camera, renderer.domElement);
controls.enableDamping = true;
controls.dampingFactor = 0.06;
controls.minDistance = 5;
controls.maxDistance = 1000;
controls.maxPolarAngle = Math.PI / 2 + 0.1;
controls.mouseButtons = {
LEFT: THREE.MOUSE.ROTATE,
MIDDLE: THREE.MOUSE.PAN,
RIGHT: THREE.MOUSE.DOLLY,
};
controls.target.set(START_X, 0, START_Z);
controls.update();
const gridHelper = new THREE.GridHelper(600, 60, 0x1a1a33, 0x151528);
gridHelper.position.y = -3;
gridHelper.visible = false;
scene.add(gridHelper);
const groundPlaneFloor = new THREE.Mesh(
new THREE.PlaneGeometry(1200, 1200),
new THREE.MeshBasicMaterial({ color: 0x12121a, side: THREE.DoubleSide })
);
groundPlaneFloor.rotation.x = -Math.PI / 2;
groundPlaneFloor.position.y = 0;
scene.add(groundPlaneFloor);
scene.add(new THREE.AmbientLight(0xffffff, 2.0));
const dirLight = new THREE.DirectionalLight(0xffffff, 1.5);
dirLight.position.set(100, 200, 100);
scene.add(dirLight);
// ──────────────────────────────────────────────────────
// Layer management
// ──────────────────────────────────────────────────────
const layerObjects = {};
const layerState = { pointcloud: false, building: false, tree: false, road: true, crosswalk: true, stopline: true, trafficlight: true, parking: true };
const trafficLights = [];
// TL animation constants
const TL_COLORS = [0xff1100, 0x00dd44, 0xffaa00];
const TL_DARK = 0x1a1a1a;
const TL_DURATIONS = [15, 15, 1.5];
const TL_NEXT = [1, 2, 0];
function applyLightColor(tl) {
const bulbs = tl.mesh.children.filter(c => c.isMesh);
const stateToSlot = [0, 2, 1];
bulbs.forEach((b, i) => {
const on = (i === stateToSlot[tl.state]);
b.material.color.setHex(on ? TL_COLORS[tl.state] : TL_DARK);
});
}
function updateTrafficLights(dt) {
const seen = new Set();
for (const tl of trafficLights) {
if (!seen.has(tl.id)) {
seen.add(tl.id);
tl.timer += dt;
if (tl.timer >= tl.duration) {
tl.timer = 0;
tl.state = TL_NEXT[tl.state];
tl.duration = TL_DURATIONS[tl.state];
}
} else {
const master = trafficLights.find(t => t.id === tl.id);
tl.state = master.state; tl.timer = master.timer; tl.duration = master.duration;
}
applyLightColor(tl);
}
}
// HUD state
let hudSpeed = 0;
let hudSteeringAngle = 0;
let hudTurnState = 0;
let hudBlinkerT = 0;
let hudPrevHeading = null;
let hudSpeedLimit = null;
let poseInfoExpanded = false;
window.togglePoseInfo = function() {
poseInfoExpanded = !poseInfoExpanded;
document.getElementById('pose-info').style.display = poseInfoExpanded ? 'block' : 'none';
const btn = document.getElementById('pose-info-toggle');
btn.style.color = poseInfoExpanded ? '#ffaa44' : '#445';
btn.style.background = poseInfoExpanded ? 'rgba(30,30,60,0.95)' : 'rgba(10,10,30,0.88)';
};
let colorbarExpanded = false;
window.toggleColorbar = function() {
colorbarExpanded = !colorbarExpanded;
document.getElementById('colorbar').style.display = colorbarExpanded ? 'block' : 'none';
document.getElementById('colorbar-toggle').textContent = colorbarExpanded ? '▼' : '▲';
};
let panelExpanded = false;
window.togglePanel = function() {
panelExpanded = !panelExpanded;
document.getElementById('panel').style.display = panelExpanded ? 'block' : 'none';
const tog = document.getElementById('panel-toggle');
tog.style.color = panelExpanded ? '#fff' : '#7fbbff';
tog.style.background = panelExpanded ? 'rgba(30,30,80,0.95)' : 'rgba(10,10,30,0.88)';
const hud = document.getElementById('vehicle-hud');
if (hud) hud.style.left = panelExpanded ? '264px' : '12px';
};
function setText(id, value) {
const el = document.getElementById(id);
if (el) el.textContent = value;
}
window.toggleLayer = function(name) {
layerState[name] = !layerState[name];
const chk = document.getElementById(`chk-${name}`);
if (chk) chk.className = `layer-check ${layerState[name] ? 'on' : ''}`;
if (USE_ROS_MAP && name === 'pointcloud') {
layerState.pointcloud = false;
if (chk) chk.className = 'layer-check';
removeLayerObject('pointcloud');
setText('s-points', 'hidden');
document.getElementById('colorbar-wrap').style.display = 'none';
return;
}
// PCD는 옵션입니다. 기본 로딩하지 않고, 체크할 때만 lazy load 합니다.
if (name === 'pointcloud' && layerState[name] && !layerObjects.pointcloud) {
setPlanStatus('PointCloud 로딩 중...');
loadPointCloud()
.then(() => {
if (layerObjects.pointcloud) layerObjects.pointcloud.visible = true;
setPlanStatus('PointCloud 로딩 완료');
})
.catch((e) => {
layerState.pointcloud = false;
if (chk) chk.className = 'layer-check';
setPlanStatus('PointCloud 로딩 실패: ' + e.message);
});
return;
}
if (layerObjects[name]) layerObjects[name].visible = layerState[name];
};
let polarGridVisible = ROBOT_STACK !== 'nav2';
window.togglePolarGrid = function() {
if (ROBOT_STACK === 'nav2') return;
polarGridVisible = !polarGridVisible;
const chk = document.getElementById('chk-polargrid');
if (chk) chk.className = `layer-check ${polarGridVisible ? 'on' : ''}`;
if (polarGrid) polarGrid.visible = polarGridVisible;
};
// ──────────────────────────────────────────────────────
// Progress
// ──────────────────────────────────────────────────────
function setProgress(pct, msg) {
// Loading state is represented by a single spinner.
}
// ──────────────────────────────────────────────────────
// Height color
// ──────────────────────────────────────────────────────
function heightColor(t) {
const stops = [[0,0,0,1],[0.25,0,1,1],[0.5,0,1,0],[0.75,1,1,0],[1,1,0,0]];
for (let i = 0; i < stops.length - 1; i++) {
if (t <= stops[i+1][0]) {
const f = (t - stops[i][0]) / (stops[i+1][0] - stops[i][0]);
return [stops[i][1]+(stops[i+1][1]-stops[i][1])*f, stops[i][2]+(stops[i+1][2]-stops[i][2])*f, stops[i][3]+(stops[i+1][3]-stops[i][3])*f];
}
}
return [1,0,0];
}
// ──────────────────────────────────────────────────────
// WGS84 lat/lon → UTM → MGRS local coordinates
// For this map: MGRS grid 54SVE
// Autoware PCD uses local coordinates inside the 100km MGRS square:
// local_x = utm_easting - 400000
// local_y = utm_northing - 3900000
// ──────────────────────────────────────────────────────
function latLonToUtm(latDeg, lonDeg, zone = UTM_ZONE) {
const a = 6378137.0;
const f = 1 / 298.257223563;
const k0 = 0.9996;
const e = Math.sqrt(f * (2 - f));
const e2 = e * e;
const ep2 = e2 / (1 - e2);
const lat = latDeg * Math.PI / 180;
const lon = lonDeg * Math.PI / 180;
const lon0 = ((zone - 1) * 6 - 180 + 3) * Math.PI / 180;
const sinLat = Math.sin(lat);
const cosLat = Math.cos(lat);
const tanLat = Math.tan(lat);
const N = a / Math.sqrt(1 - e2 * sinLat * sinLat);
const T = tanLat * tanLat;
const C = ep2 * cosLat * cosLat;
const A = cosLat * (lon - lon0);
const M = a * (
(1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256) * lat
- (3 * e2 / 8 + 3 * e2 * e2 / 32 + 45 * e2 * e2 * e2 / 1024) * Math.sin(2 * lat)
+ (15 * e2 * e2 / 256 + 45 * e2 * e2 * e2 / 1024) * Math.sin(4 * lat)
- (35 * e2 * e2 * e2 / 3072) * Math.sin(6 * lat)
);
const easting = k0 * N * (
A + (1 - T + C) * Math.pow(A, 3) / 6
+ (5 - 18 * T + T * T + 72 * C - 58 * ep2) * Math.pow(A, 5) / 120
) + 500000.0;
let northing = k0 * (
M + N * tanLat * (
A * A / 2
+ (5 - T + 9 * C + 4 * C * C) * Math.pow(A, 4) / 24
+ (61 - 58 * T + T * T + 600 * C - 330 * ep2) * Math.pow(A, 6) / 720
)
);
if (latDeg < 0) northing += 10000000.0;
return { easting, northing };
}
function nodeToLocalXY(node) {
let lx = NaN, ly = NaN, ele = 0;
for (const tag of node.querySelectorAll('tag')) {
const k = tag.getAttribute('k');
const raw = tag.getAttribute('v');
if (k === 'local_x') lx = parseFloat(raw);
else if (k === 'local_y') ly = parseFloat(raw);
else if (k === 'ele') ele = parseFloat(raw);
}
// Some Lanelet2 maps only have lat/lon + mgrs_code.
// Convert lat/lon to UTM and then to MGRS-local coordinates.
if (!Number.isFinite(lx) || !Number.isFinite(ly)) {
const lat = parseFloat(node.getAttribute('lat'));
const lon = parseFloat(node.getAttribute('lon'));
if (Number.isFinite(lat) && Number.isFinite(lon)) {
const utm = latLonToUtm(lat, lon, UTM_ZONE);
lx = utm.easting - MGRS_EASTING_BASE;
ly = utm.northing - MGRS_NORTHING_BASE;
}
}
return { lx, ly, ele };
}
// ──────────────────────────────────────────────────────
// Load PCD
// ──────────────────────────────────────────────────────
async function loadPointCloud() {
setProgress(5, 'Fetching point cloud (5MB)...');
const resp = await fetch(`${MAP_DIR}/pointcloud_map.pcd`);
if (!resp.ok) throw new Error(`PCD fetch failed: ${resp.status}`);
const buffer = await resp.arrayBuffer();
setProgress(30, 'Parsing PCD header...');
const bytes = new Uint8Array(buffer);
const marker = [68,65,84,65,32,98,105,110,97,114,121,10];
let dataOffset = -1;
for (let i = 0; i < bytes.length - marker.length; i++) {
let ok = true;
for (let j = 0; j < marker.length; j++) { if (bytes[i+j] !== marker[j]) { ok=false; break; } }
if (ok) { dataOffset = i + marker.length; break; }
}
if (dataOffset < 0) throw new Error('PCD DATA marker not found');
const headerText = new TextDecoder().decode(bytes.slice(0, dataOffset));
const numPoints = parseInt(headerText.match(/POINTS\s+(\d+)/)[1]);
setProgress(35, `Building ${numPoints.toLocaleString()} points...`);
const stride = 16;
const dv = new DataView(buffer, dataOffset);
let zMin = Infinity, zMax = -Infinity;
for (let i = 0; i < numPoints; i++) {
const z = dv.getFloat32(i*stride+8, true);
if (z < zMin) zMin = z; if (z > zMax) zMax = z;
}
const zRange = zMax - zMin || 1;
const positions = new Float32Array(numPoints * 3);
const colors = new Float32Array(numPoints * 3);
for (let i = 0; i < numPoints; i++) {
const px = dv.getFloat32(i*stride+0, true);
const py = dv.getFloat32(i*stride+4, true);
const pz = dv.getFloat32(i*stride+8, true);
const Z_OFFSET = 0;
positions[i*3+0] = (px - CENTER_X);
positions[i*3+1] = pz + Z_OFFSET;
positions[i*3+2] = -(py - CENTER_Y);
const [r,g,b] = heightColor((pz - zMin) / zRange);
colors[i*3+0]=r; colors[i*3+1]=g; colors[i*3+2]=b;
}
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(positions, 3));
geom.setAttribute('color', new THREE.BufferAttribute(colors, 3));
const pts = new THREE.Points(geom, new THREE.PointsMaterial({ size: 0.18, vertexColors: true, sizeAttenuation: true }));
scene.add(pts);
layerObjects.pointcloud = pts;
pts.visible = layerState.pointcloud;
document.getElementById('cb-max').textContent = zMax.toFixed(1);
pcdZMin = zMin; pcdZMax = zMax;
document.getElementById('cb-min').textContent = zMin.toFixed(1);
setText('s-points', numPoints.toLocaleString());
setProgress(60, 'Point cloud loaded.');
}
function getPayloadCenter(payload) {
if (payload?.center && Number.isFinite(payload.center.x) && Number.isFinite(payload.center.y)) {
return { x: payload.center.x, y: payload.center.y };
}
return rosMapCenter || { x: CENTER_X, y: CENTER_Y };
}
function getRosInitialCameraHeight(payload) {
if (ROBOT_STACK !== 'nav2') return Math.max(camera.position.y, 80);
const width = Number(payload?.width) || 0;
const height = Number(payload?.height) || 0;
const resolution = Number(payload?.resolution) || 1;
const mapSize = Math.max(width * resolution, height * resolution);
if (!Number.isFinite(mapSize) || mapSize <= 0) return 12;
return Math.max(5, Math.min(24, mapSize * 1.1));
}
function updateRosMapCenter(payload) {
// Prefer vector map center for the camera. Some maps publish vector markers
// and pointclouds in different local frames; merging their bounds can push
// one layer far outside the view.
rosMapCenter = getPayloadCenter(rosVectorPayload || payload);
roadElevation = 0;
groundPlane.constant = 0;
groundPlaneFloor.position.y = 0;
controls.target.set(0, 0, 0);
camera.position.set(0, getRosInitialCameraHeight(payload), 0.01);
controls.update();
console.log('[ROS map center]', rosMapCenter, { payloadCenter: payload?.center, payloadType: payload?.type });
}
function getPayloadGround(payload) {
const zMin = payload?.bounds?.z_min;
return Number.isFinite(zMin) ? zMin : 0;
}
function boundsSize(bounds) {
if (!bounds) return null;
const w = bounds.x_max - bounds.x_min;
const h = bounds.y_max - bounds.y_min;
if (!Number.isFinite(w) || !Number.isFinite(h) || Math.abs(w) < 1e-3 || Math.abs(h) < 1e-3) return null;
return { w: Math.abs(w), h: Math.abs(h) };
}
function getRosRenderTransform(payload) {
const center = getPayloadCenter(payload) || { x: CENTER_X, y: CENTER_Y };
const transform = { center, scaleX: 1, scaleY: 1 };
if (!USE_ROS_MAP || payload?.type !== 'pointcloud' || !rosVectorPayload?.bounds || !payload?.bounds) return transform;
const vectorSize = boundsSize(rosVectorPayload.bounds);
const pcSize = boundsSize(payload.bounds);
if (!vectorSize || !pcSize) return transform;
// If pointcloud and vector map come from different local origins/projectors,
// fit the pointcloud XY extent onto the vector-map extent. This keeps PCD
// visually aligned with the lane geometry while preserving each layer's z.
transform.scaleX = vectorSize.w / pcSize.w;
transform.scaleY = vectorSize.h / pcSize.h;
transform.scaleX = Math.min(10, Math.max(0.1, transform.scaleX));
transform.scaleY = Math.min(10, Math.max(0.1, transform.scaleY));
return transform;
}
function mapPayloadPointToThree(p, payload = null, yOffset = 0) {
const t = getRosRenderTransform(payload);
const ground = getPayloadGround(payload);
return [
(p[0] - t.center.x) * t.scaleX,
payload?.type === 'vector_map' ? roadElevation + yOffset : (p[2] - ground) + yOffset,
-((p[1] - t.center.y) * t.scaleY),
];
}
function nav2MapPoseToThree(p, center = getPayloadCenter(rosVectorPayload)) {
return [
Number(p.x) - center.x,
Number(p.z) || 0,
-(Number(p.y) - center.y),
Number(p.t) || 0,
];
}
function headingToRosQuat(headingDeg) {
const headingRad = (Number(headingDeg) || 0) * Math.PI / 180;
const yaw = Math.PI / 2 - headingRad;
return { qx: 0, qy: 0, qz: Math.sin(yaw / 2), qw: Math.cos(yaw / 2) };
}
function threePoseToRosPayload(pose) {
const center = USE_ROS_MAP ? getPayloadCenter(rosVectorPayload) : { x: CENTER_X, y: CENTER_Y };
const q = headingToRosQuat(pose.heading);
return {
x: pose.pos.x + center.x,
y: center.y - pose.pos.z,
z: pose.pos.y,
...q,
};
}
function markerPointToMapPoint(marker, point) {
const pose = marker.pose || {};
const pos = pose.position || { x: 0, y: 0, z: 0 };
const q = pose.orientation || { x: 0, y: 0, z: 0, w: 1 };
const v = new THREE.Vector3(point[0] || 0, point[1] || 0, point[2] || 0);
v.applyQuaternion(new THREE.Quaternion(q.x || 0, q.y || 0, q.z || 0, q.w || 1));
return [v.x + (pos.x || 0), v.y + (pos.y || 0), v.z + (pos.z || 0)];
}
function renderAllRosMapLayers() {
if (!USE_ROS_MAP && rosPointcloudPayload && rosPointcloudAcceptedAfterVector) renderPointCloudPayload(rosPointcloudPayload);
if (rosVectorPayload) renderVectorMapPayload(rosVectorPayload);
}
function renderOccupancyGridPayload(payload) {
if (!payload || !Array.isArray(payload.data)) return;
removeLayerObject('rosVectorMap');
removeLayerObject('road');
removeLayerObject('crosswalk');
removeLayerObject('stopline');
removeLayerObject('trafficlight');
removeLayerObject('parking');
removeLayerObject('building');
removeLayerObject('tree');
const width = payload.width | 0;
const height = payload.height | 0;
const resolution = Number(payload.resolution) || 1;
if (width <= 0 || height <= 0 || payload.data.length < width * height) return;
const canvas = document.createElement('canvas');
canvas.width = width;
canvas.height = height;
const ctx = canvas.getContext('2d');
const img = ctx.createImageData(width, height);
for (let y = 0; y < height; y++) {
for (let x = 0; x < width; x++) {
const srcIdx = y * width + x;
const dstIdx = ((height - 1 - y) * width + x) * 4;
const value = payload.data[srcIdx];
let shade = 34;
let alpha = 0;
if (value < 0) {
shade = 70;
alpha = 55;
} else if (value >= 65) {
shade = 235;
alpha = 120;
} else {
shade = 48;
alpha = 45;
}
img.data[dstIdx + 0] = shade;
img.data[dstIdx + 1] = shade;
img.data[dstIdx + 2] = shade;
img.data[dstIdx + 3] = alpha;
}
}
ctx.putImageData(img, 0, 0);
const texture = new THREE.CanvasTexture(canvas);
texture.colorSpace = THREE.SRGBColorSpace;
texture.needsUpdate = true;
const material = new THREE.MeshBasicMaterial({
map: texture,
transparent: true,
side: THREE.DoubleSide,
depthWrite: false,
});
const geom = new THREE.PlaneGeometry(width * resolution, height * resolution);
const mesh = new THREE.Mesh(geom, material);
mesh.rotation.x = -Math.PI / 2;
const origin = payload.origin || { x: 0, y: 0, z: 0 };
const cx = origin.x + width * resolution * 0.5;
const cy = origin.y + height * resolution * 0.5;
const [mx, my, mz] = mapPayloadPointToThree([cx, cy, origin.z || 0], payload, 0.02);
mesh.position.set(mx, my, mz);
scene.add(mesh);
layerObjects.rosVectorMap = mesh;
layerObjects.road = mesh;
mesh.visible = layerState.road;
document.getElementById('s-road').textContent = `${width} x ${height}`;
document.getElementById('s-cross').textContent = '-';
setText('s-tl', '-');
document.getElementById('s-parking').textContent = '-';
setPlanStatus(`Nav2 맵 수신 완료: ${width} x ${height}`);
}
function renderPointCloudPayload(payload) {
if (!payload || !Array.isArray(payload.points)) return;
removeLayerObject('pointcloud');
const points = payload.points;
console.log('[ROS pointcloud transform]', getRosRenderTransform(payload), { pointcloudBounds: payload.bounds, vectorBounds: rosVectorPayload?.bounds });
const positions = new Float32Array(points.length * 3);
const colors = new Float32Array(points.length * 3);
const zMin = Number.isFinite(payload.z_min) ? payload.z_min : 0;
const zMax = Number.isFinite(payload.z_max) ? payload.z_max : 1;
const zRange = zMax - zMin || 1;
for (let i = 0; i < points.length; i++) {
const [x, y, z] = payload.coordinate_frame === 'map' ? mapPayloadPointToThree(points[i], payload, 0) : points[i];
positions[i * 3 + 0] = x;
positions[i * 3 + 1] = y;
positions[i * 3 + 2] = z;
const rawZ = Array.isArray(points[i]) ? points[i][2] : y;
const [r, g, b] = heightColor((rawZ - zMin) / zRange);
colors[i * 3 + 0] = r;
colors[i * 3 + 1] = g;
colors[i * 3 + 2] = b;
}
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(positions, 3));
geom.setAttribute('color', new THREE.BufferAttribute(colors, 3));
const pts = new THREE.Points(geom, new THREE.PointsMaterial({ size: 0.18, vertexColors: true, sizeAttenuation: true }));
scene.add(pts);
layerObjects.pointcloud = pts;
layerState.pointcloud = true;
pts.visible = true;
const chk = document.getElementById('chk-pointcloud');
if (chk) chk.className = 'layer-check on';
setText('s-points', `${points.length.toLocaleString()} / ${(payload.source_count || points.length).toLocaleString()}`);
document.getElementById('cb-min').textContent = zMin.toFixed(1);
document.getElementById('cb-max').textContent = zMax.toFixed(1);
pcdZMin = zMin;
pcdZMax = zMax;
document.getElementById('colorbar-wrap').style.display = 'block';
}
// ──────────────────────────────────────────────────────
// Load Lanelet2
// ──────────────────────────────────────────────────────
function makeLineMat(color, opacity=1) {
return new THREE.LineBasicMaterial({ color, transparent: opacity<1, opacity,
polygonOffset: true, polygonOffsetFactor: -1, polygonOffsetUnits: -1 });
}
function removeLayerObject(name) {
const obj = layerObjects[name];
if (!obj) return;
scene.remove(obj);
obj.traverse?.((child) => {
child.geometry?.dispose?.();
if (Array.isArray(child.material)) child.material.forEach((m) => m?.dispose?.());
else child.material?.dispose?.();
});
for (const key of Object.keys(layerObjects)) {
if (layerObjects[key] === obj) layerObjects[key] = null;
}
}
function markerMaterial(marker, fallback = 0x88ccff) {
const c = Array.isArray(marker.color) ? marker.color : null;
const color = c ? new THREE.Color(c[0], c[1], c[2]) : new THREE.Color(fallback);
const sourceOpacity = c && Number.isFinite(c[3]) ? c[3] : 0.9;
const opacity = USE_ROS_MAP ? Math.min(0.38, Math.max(0.12, sourceOpacity)) : Math.max(0.18, sourceOpacity);
return new THREE.LineBasicMaterial({
color,
transparent: opacity < 1,
opacity,
depthWrite: !USE_ROS_MAP,
});
}
function markerPointsToVectors(marker, yLift = 0.06, payload = null) {
return (marker.points || []).map((p) => {
const [x, y, z] = mapPayloadPointToThree(markerPointToMapPoint(marker, p), payload, yLift);
return new THREE.Vector3(x, y, z);
});
}
function markerPoseVector(marker, yLift = 0.06, payload = null) {
const pos = marker.pose?.position || { x: 0, y: 0, z: 0 };
const [x, y, z] = mapPayloadPointToThree([pos.x, pos.y, pos.z], payload, yLift);
return new THREE.Vector3(x, y, z);
}
function markerPoseQuaternion(marker) {
const q = marker.pose?.orientation || { x: 0, y: 0, z: 0, w: 1 };
return new THREE.Quaternion(q.x || 0, q.y || 0, q.z || 0, q.w || 1);
}
function markerMeshMaterial(marker, fallback = 0x88ccff) {
const c = Array.isArray(marker.color) ? marker.color : null;
const color = c ? new THREE.Color(c[0], c[1], c[2]) : new THREE.Color(fallback);
const sourceOpacity = c && Number.isFinite(c[3]) ? c[3] : 0.85;
const opacity = USE_ROS_MAP ? Math.min(0.28, Math.max(0.10, sourceOpacity)) : Math.max(0.18, sourceOpacity);
return new THREE.MeshBasicMaterial({
color,
transparent: opacity < 1,
opacity,
side: THREE.DoubleSide,
depthWrite: !USE_ROS_MAP && opacity >= 0.75,
});
}
function addPoseMarkerMesh(group, marker) {
const scale = marker.scale || {};
const sx = Math.max(0.05, scale.x || 0.4);
const sy = Math.max(0.05, scale.y || 0.4);
const sz = Math.max(0.05, scale.z || 0.4);
let geom = null;
if (marker.type === 1) geom = new THREE.BoxGeometry(sx, sz, sy); // CUBE: map x/y/z -> three x/z/y
else if (marker.type === 2) geom = new THREE.SphereGeometry(Math.max(sx, sy, sz) / 2, 12, 8);
else if (marker.type === 3) geom = new THREE.CylinderGeometry(sx / 2, sx / 2, Math.max(0.05, sz), 12);
if (!geom) return false;
const mesh = new THREE.Mesh(geom, markerMeshMaterial(marker));
mesh.position.copy(markerPoseVector(marker, 0.06, marker.__payload));
mesh.quaternion.copy(markerPoseQuaternion(marker));
group.add(mesh);
return true;
}
function addTriangleList(group, marker, pts) {
if (pts.length < 3) return false;
const positions = new Float32Array(pts.length * 3);
for (let i = 0; i < pts.length; i++) {
positions[i * 3 + 0] = pts[i].x;
positions[i * 3 + 1] = pts[i].y;
positions[i * 3 + 2] = pts[i].z;
}
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(positions, 3));
geom.computeVertexNormals();
group.add(new THREE.Mesh(geom, markerMeshMaterial(marker, 0x44ff88)));
return true;
}
function renderVectorMapPayload(payload) {
if (!payload || !Array.isArray(payload.markers)) return;
removeLayerObject('rosVectorMap');
removeLayerObject('road');
removeLayerObject('crosswalk');
removeLayerObject('stopline');
removeLayerObject('trafficlight');
removeLayerObject('parking');
removeLayerObject('building');
removeLayerObject('tree');
trafficLights.length = 0;
laneletList.length = 0;
laneletGraph.clear();
const group = new THREE.Group();
group.name = 'ROS Vector Map';
let lineCount = 0;
let pointCount = 0;
let meshCount = 0;
const typeStats = {};
for (const marker of payload.markers) {
if (marker.action === 2 || marker.action === 3) continue;
typeStats[marker.type] = (typeStats[marker.type] || 0) + 1;
marker.__payload = payload;
const pts = markerPointsToVectors(marker, 0.06, payload);
const mat = markerMaterial(marker);
if (marker.type === 1 || marker.type === 2 || marker.type === 3) { // CUBE/SPHERE/CYLINDER
if (addPoseMarkerMesh(group, marker)) meshCount++;
} else if (marker.type === 4) { // LINE_STRIP
if (pts.length >= 2) {
group.add(new THREE.Line(new THREE.BufferGeometry().setFromPoints(pts), mat));
lineCount++;
}
} else if (marker.type === 5) { // LINE_LIST
for (let i = 0; i + 1 < pts.length; i += 2) {
group.add(new THREE.Line(new THREE.BufferGeometry().setFromPoints([pts[i], pts[i + 1]]), mat.clone()));
lineCount++;
}
} else if (marker.type === 8) { // POINTS
if (pts.length === 0) continue;
const positions = new Float32Array(pts.length * 3);
for (let i = 0; i < pts.length; i++) {
positions[i * 3 + 0] = pts[i].x;
positions[i * 3 + 1] = pts[i].y;
positions[i * 3 + 2] = pts[i].z;
}
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(positions, 3));
const color = Array.isArray(marker.color) ? new THREE.Color(marker.color[0], marker.color[1], marker.color[2]) : new THREE.Color(0xffaa00);
const points = new THREE.Points(geom, new THREE.PointsMaterial({ color, size: Math.max(0.08, marker.scale?.x || 0.16), sizeAttenuation: true }));
group.add(points);
pointCount += pts.length;
} else if (marker.type === 11) { // TRIANGLE_LIST
if (addTriangleList(group, marker, pts)) meshCount++;
} else if (pts.length >= 2) {
group.add(new THREE.Line(new THREE.BufferGeometry().setFromPoints(pts), mat));
lineCount++;
}
}
console.log('[ROS vector marker types]', typeStats);
scene.add(group);
layerObjects.rosVectorMap = group;
layerObjects.road = group;
group.visible = layerState.road;
document.getElementById('s-road').textContent = lineCount.toLocaleString();
document.getElementById('s-cross').textContent = '-';
setText('s-tl', pointCount ? pointCount.toLocaleString() : '-');
document.getElementById('s-parking').textContent = '-';
setPlanStatus(`ROS 맵 수신 완료: vector ${lineCount.toLocaleString()} lines, ${meshCount.toLocaleString()} meshes`);
}
function connectRosMapBridge() {
return new Promise((resolve) => {
let settled = false;
const done = () => {
if (settled) return;
settled = true;
resolve();
};
const ws = new WebSocket(`${ROS_BRIDGE_WS}/ws/map`);
const timeout = setTimeout(done, 10000);
ws.onopen = () => setPlanStatus('ROS 맵 토픽 대기 중...');
ws.onmessage = (event) => {
const payload = JSON.parse(event.data);
if (payload.type === 'stack_mismatch') {
setPlanStatus(`브리지 stack 불일치: 화면 ${payload.requested}, 브리지 ${payload.active}`);
clearTimeout(timeout);
done();
return;
}
if (payload.type === 'pointcloud') {
if (USE_ROS_MAP) {
rosPointcloudPayload = null;
rosPointcloudAcceptedAfterVector = false;
removeLayerObject('pointcloud');
layerState.pointcloud = false;
setText('s-points', 'hidden');
document.getElementById('colorbar-wrap').style.display = 'none';
clearTimeout(timeout);
done();
return;
}
rosPointcloudPayload = payload;
rosPointcloudAcceptedAfterVector = Boolean(rosVectorPayload);
if (!rosPointcloudAcceptedAfterVector) {
removeLayerObject('pointcloud');
setText('s-points', 'waiting vector');
}
updateRosMapCenter(payload);
renderAllRosMapLayers();
}
if (payload.type === 'vector_map') {
const revisionChanged = rosVectorRevision !== payload.revision;
rosVectorRevision = payload.revision;
rosVectorPayload = payload;
if (revisionChanged) {
rosPointcloudPayload = null;
rosPointcloudAcceptedAfterVector = false;
removeLayerObject('pointcloud');
setText('s-points', 'hidden');
document.getElementById('colorbar-wrap').style.display = 'none';
}
updateRosMapCenter(payload);
renderAllRosMapLayers();
}
if (payload.type === 'occupancy_grid' || payload.type === 'nav2_map') {
rosVectorRevision = payload.revision;
rosVectorPayload = payload;
rosPointcloudPayload = null;
rosPointcloudAcceptedAfterVector = false;
removeLayerObject('pointcloud');
setText('s-points', 'hidden');
document.getElementById('colorbar-wrap').style.display = 'none';
updateRosMapCenter(payload);
renderOccupancyGridPayload(payload);
}
clearTimeout(timeout);
done();
};
ws.onerror = () => {
setPlanStatus('ROS 맵 브리지 연결 실패');
clearTimeout(timeout);
done();
};
ws.onclose = () => {
if (!settled) setPlanStatus('ROS 맵 브리지 연결 종료');
clearTimeout(timeout);
done();
};
window.__rosMapWs = ws;
});
}
function connectNav2VisualizationBridge() {
if (ROBOT_STACK !== 'nav2' || window.__nav2VizWs) return;
try {
const ws = new WebSocket(`${ROS_BRIDGE_WS}/ws/nav2`);
ws.onmessage = (event) => {
const payload = JSON.parse(event.data);
if (payload.type === 'stack_mismatch') {
setPlanStatus(`브리지 stack 불일치: 화면 ${payload.requested}, 브리지 ${payload.active}`);
return;
}
if (payload.type === 'nav2_path' && Array.isArray(payload.poses)) {
if (!poseState.goal) return;
plannedPts = [];
if (plannedLine) { scene.remove(plannedLine); plannedLine = null; }
if (routeLaneletBorder) { scene.remove(routeLaneletBorder); routeLaneletBorder = null; }
saveViewerState();
}
};
ws.onclose = () => {
window.__nav2VizWs = null;
window.setTimeout(connectNav2VisualizationBridge, 1500);
};
ws.onerror = () => {
ws.close();
};
window.__nav2VizWs = ws;
} catch {}
}
function wayToVectors(nodeMap, wayMap, wayId) {
return (wayMap.get(wayId) || []).map(r => nodeMap.get(r)).filter(Boolean);
}
const MAX_SEGMENT = 200; // m 이상 세그먼트는 비정상 데이터로 제외
function buildLine(pts, mat, yLift=0.04) {
if (pts.length < 2) return null;
for (let i = 0; i < pts.length - 1; i++) {
const dx = pts[i+1].x - pts[i].x, dz = pts[i+1].z - pts[i].z;
if (Math.sqrt(dx*dx + dz*dz) > MAX_SEGMENT) return null;
}
const lifted = pts.map(p => new THREE.Vector3(p.x, p.y + yLift, p.z));
return new THREE.Line(new THREE.BufferGeometry().setFromPoints(lifted), mat);
}
function buildLineMesh(pts, color, opacity=1, yLift=0.05, halfW=0.03) {
if (pts.length < 2) return null;
const verts = [];
for (let i = 0; i < pts.length - 1; i++) {
const a = pts[i], b = pts[i+1];
const dx = b.x - a.x, dz = b.z - a.z;
const len = Math.sqrt(dx*dx + dz*dz);
if (len > MAX_SEGMENT) return null;
const rx = -dz / len * halfW, rz = dx / len * halfW;
const ya = a.y + yLift, yb = b.y + yLift;
verts.push(
a.x-rx, ya, a.z-rz, a.x+rx, ya, a.z+rz, b.x-rx, yb, b.z-rz,
b.x-rx, yb, b.z-rz, a.x+rx, ya, a.z+rz, b.x+rx, yb, b.z+rz
);
}
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(new Float32Array(verts), 3));
const mesh = new THREE.Mesh(geom, new THREE.MeshBasicMaterial({
color, side: THREE.DoubleSide, transparent: opacity < 1, opacity,
depthWrite: false,
polygonOffset: true, polygonOffsetFactor: -1, polygonOffsetUnits: -1,
}));
mesh.renderOrder = 1;
return mesh;
}
function buildLaneletMesh(leftPts, rightPts, color, opacity=0.18, depthWrite=false, yOffset=0) {
const nL = leftPts.length, nR = rightPts.length;
if (nL < 2 || nR < 2) return null;
for (let i = 0; i < nL-1; i++) {
const dx=leftPts[i+1].x-leftPts[i].x, dz=leftPts[i+1].z-leftPts[i].z;
if (Math.sqrt(dx*dx+dz*dz) > MAX_SEGMENT) return null;
}
for (let i = 0; i < nR-1; i++) {
const dx=rightPts[i+1].x-rightPts[i].x, dz=rightPts[i+1].z-rightPts[i].z;
if (Math.sqrt(dx*dx+dz*dz) > MAX_SEGMENT) return null;
}
const n = Math.max(nL, nR);
const verts = [];
for (let i = 0; i < n-1; i++) {
const l0=leftPts[Math.min(i, nL-1)], l1=leftPts[Math.min(i+1, nL-1)];
const r0=rightPts[Math.min(i, nR-1)], r1=rightPts[Math.min(i+1, nR-1)];
verts.push(l0.x,l0.y+yOffset,l0.z, r0.x,r0.y+yOffset,r0.z, l1.x,l1.y+yOffset,l1.z);
verts.push(l1.x,l1.y+yOffset,l1.z, r0.x,r0.y+yOffset,r0.z, r1.x,r1.y+yOffset,r1.z);
}
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(new Float32Array(verts), 3));
const mat = new THREE.MeshBasicMaterial({ color, side: THREE.DoubleSide, transparent: opacity<1, opacity, depthWrite,
polygonOffset: true, polygonOffsetFactor: 1, polygonOffsetUnits: 1 });
return new THREE.Mesh(geom, mat);
}
function buildParkingLotMesh(pts, width) {
const a = pts[0], b = pts[1];
const dx = b.x - a.x, dz = b.z - a.z;
const len = Math.sqrt(dx * dx + dz * dz) || 1;
const rx = -dz / len * (width / 2), rz = dx / len * (width / 2);
const y = (a.y + b.y) / 2 + 0.04;
const verts = new Float32Array([
a.x - rx, y, a.z - rz, a.x + rx, y, a.z + rz, b.x - rx, y, b.z - rz,
b.x - rx, y, b.z - rz, a.x + rx, y, a.z + rz, b.x + rx, y, b.z + rz,
]);
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(verts, 3));
return new THREE.Mesh(geom, new THREE.MeshBasicMaterial({
color: 0x9c6b12, side: THREE.DoubleSide,
transparent: true, opacity: 0.45, depthWrite: false,
}));
}
function buildStopLine(pts, halfW=0.2) {
if (pts.length < 2) return null;
const verts = [];
for (let i = 0; i < pts.length - 1; i++) {
const a = pts[i], b = pts[i + 1];
const dx = b.x - a.x, dz = b.z - a.z;
const len = Math.sqrt(dx*dx + dz*dz);
if (len > 30) return null;
const rx = -dz / len * halfW, rz = dx / len * halfW;
const y = a.y + 0.06, yb = b.y + 0.06;
verts.push(
a.x-rx, y, a.z-rz, a.x+rx, y, a.z+rz, b.x-rx, yb, b.z-rz,
b.x-rx, yb, b.z-rz, a.x+rx, y, a.z+rz, b.x+rx, yb, b.z+rz
);
}
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(new Float32Array(verts), 3));
const mesh = new THREE.Mesh(geom, new THREE.MeshBasicMaterial({ color: 0x666666, side: THREE.DoubleSide }));
mesh.renderOrder = 2;
return mesh;
}
function buildCrosswalkStripes(leftPts, rightPts) {
if (leftPts.length < 2 || rightPts.length < 2) return null;
const lFirst = leftPts[0];
const rFirst = rightPts[0];
const rLast = rightPts[rightPts.length - 1];
const d0 = (lFirst.x - rFirst.x) ** 2 + (lFirst.z - rFirst.z) ** 2;
const dN = (lFirst.x - rLast.x) ** 2 + (lFirst.z - rLast.z) ** 2;
const alignedRight = dN < d0 ? [...rightPts].reverse() : rightPts;
const STRIPE = 0.45;
const GAP = 0.45;
const Y_UP = 0.10;
const mat = new THREE.MeshBasicMaterial({
color: 0x555555,
side: THREE.DoubleSide,
polygonOffset: true,
polygonOffsetFactor: -1,
polygonOffsetUnits: -1,
});
const group = new THREE.Group();
function interp(pts, d) {
let rem = Math.max(0, d);
for (let i = 0; i < pts.length - 1; i++) {
const dx = pts[i + 1].x - pts[i].x;
const dy = pts[i + 1].y - pts[i].y;
const dz = pts[i + 1].z - pts[i].z;
const l = Math.sqrt(dx * dx + dz * dz);
if (rem <= l + 1e-6 || i === pts.length - 2) {
const f = l > 1e-6 ? Math.min(rem / l, 1) : 0;
return {
x: pts[i].x + dx * f,
y: pts[i].y + dy * f + Y_UP,
z: pts[i].z + dz * f,
};
}
rem -= l;
}
const p = pts[pts.length - 1];
return { x: p.x, y: p.y + Y_UP, z: p.z };
}
function calcLength(pts) {
let len = 0;
for (let i = 0; i < pts.length - 1; i++) {
const dx = pts[i + 1].x - pts[i].x;
const dz = pts[i + 1].z - pts[i].z;
len += Math.sqrt(dx * dx + dz * dz);
}
return len;
}
// 중심선 생성
const n = Math.min(leftPts.length, alignedRight.length);
const centerPts = [];
for (let i = 0; i < n; i++) {
centerPts.push({
x: (leftPts[i].x + alignedRight[i].x) * 0.5,
y: (leftPts[i].y + alignedRight[i].y) * 0.5,
z: (leftPts[i].z + alignedRight[i].z) * 0.5,
});
}
const totalLen = calcLength(centerPts);
if (totalLen < 0.1 || totalLen > 200) return null;
// 횡단보도 폭 계산
let widthSum = 0;
let widthCnt = 0;
for (let i = 0; i < n; i++) {
const dx = leftPts[i].x - alignedRight[i].x;
const dz = leftPts[i].z - alignedRight[i].z;
const w = Math.sqrt(dx * dx + dz * dz);
if (w > 0.1 && w < 30) {
widthSum += w;
widthCnt++;
}
}
const halfWidth = (widthCnt > 0 ? widthSum / widthCnt : 3.0) * 0.5;
for (let d = 0; d < totalLen; d += STRIPE + GAP) {
const d1 = Math.min(d + STRIPE, totalLen);
const p0 = interp(centerPts, d);
const p1 = interp(centerPts, d1);
const dx = p1.x - p0.x;
const dz = p1.z - p0.z;
const len = Math.sqrt(dx * dx + dz * dz);
if (len < 1e-6) continue;
const nx = -dz / len;
const nz = dx / len;
const l0 = {
x: p0.x + nx * halfWidth,
y: p0.y,
z: p0.z + nz * halfWidth,
};
const r0 = {
x: p0.x - nx * halfWidth,
y: p0.y,
z: p0.z - nz * halfWidth,
};
const l1 = {
x: p1.x + nx * halfWidth,
y: p1.y,
z: p1.z + nz * halfWidth,
};
const r1 = {
x: p1.x - nx * halfWidth,
y: p1.y,
z: p1.z - nz * halfWidth,
};
const v = new Float32Array([
l0.x, l0.y, l0.z, r0.x, r0.y, r0.z, l1.x, l1.y, l1.z,
l1.x, l1.y, l1.z, r0.x, r0.y, r0.z, r1.x, r1.y, r1.z,
]);
const g = new THREE.BufferGeometry();
g.setAttribute("position", new THREE.BufferAttribute(v, 3));
group.add(new THREE.Mesh(g, mat));
}
return group;
}
function nodeToVec(n) {
return new THREE.Vector3(n.x, n.z + 0.05, -n.y);
}
async function loadLanelet2() {
setProgress(65, 'Fetching Lanelet2 map...');
const resp = await fetch(`${MAP_DIR}/lanelet2_map.osm`);
if (!resp.ok) throw new Error(`OSM fetch failed: ${resp.status}`);
const doc = new DOMParser().parseFromString(await resp.text(), 'application/xml');
setProgress(72, 'Parsing XML...');
const nodeMap = new Map();
let eleMin = Infinity;
const eleArr = [];
let osmXMin = Infinity, osmXMax = -Infinity;
let osmYMin = Infinity, osmYMax = -Infinity;
for (const node of doc.querySelectorAll('node')) {
const id = node.getAttribute('id');
const { lx, ly, ele } = nodeToLocalXY(node);
if (!Number.isFinite(lx) || !Number.isFinite(ly)) continue;
nodeMap.set(id, { x: lx - CENTER_X, y: ly - CENTER_Y, z: ele });
if (lx < osmXMin) osmXMin = lx;
if (lx > osmXMax) osmXMax = lx;
if (ly < osmYMin) osmYMin = ly;
if (ly > osmYMax) osmYMax = ly;
if (ele < eleMin) eleMin = ele;
eleArr.push(ele);
}
// 도로 기준 고도: eleMin은 저점 이상치로 너무 낮을 수 있어 중앙값 사용
if (eleArr.length > 0) {
eleArr.sort((a, b) => a - b);
const eleMedian = eleArr[Math.floor(eleArr.length / 2)];
roadElevation = eleMedian;
groundPlane.constant = -roadElevation;
groundPlaneFloor.position.y = roadElevation;
}
console.log('[Lanelet2 local bounds]', {
xMin: osmXMin, xMax: osmXMax,
yMin: osmYMin, yMax: osmYMax,
centerX: CENTER_X, centerY: CENTER_Y
});
const wayMap=new Map(), wayTypes=new Map();
for (const way of doc.querySelectorAll('way')) {
const id = way.getAttribute('id');
wayMap.set(id, Array.from(way.querySelectorAll('nd')).map(n=>n.getAttribute('ref')));
let wt='', ws='';
for (const tag of way.querySelectorAll('tag')) {
if (tag.getAttribute('k')==='type') wt=tag.getAttribute('v');
if (tag.getAttribute('k')==='subtype') ws=tag.getAttribute('v');
}
wayTypes.set(id, { type:wt, subtype:ws });
}
setProgress(80, 'Building geometry...');
const roadGroup=new THREE.Group(), crosswalkGroup=new THREE.Group();
const stoplineGroup=new THREE.Group(), tlGroup=new THREE.Group();
let roadCount=0, crossCount=0, tlCount=0;
const roadLineMat=makeLineMat(0xaaaaaa), crossLineMat=makeLineMat(0x999999,0.5);
// 경로탐색용 레인 데이터
laneletList.length = 0;
const endNodeIndex = new Map(); // "leftEndRef_rightEndRef" → laneletId[]
for (const rel of doc.querySelectorAll('relation')) {
let rtype='', rsub='';
for (const tag of rel.querySelectorAll('tag')) {
if (tag.getAttribute('k')==='type') rtype=tag.getAttribute('v');
if (tag.getAttribute('k')==='subtype') rsub=tag.getAttribute('v');
}
if (rtype!=='lanelet') continue;
const relId = rel.getAttribute('id');
let leftId=null, rightId=null;
for (const m of rel.querySelectorAll('member')) {
if (m.getAttribute('type')!=='way') continue;
if (m.getAttribute('role')==='left') leftId=m.getAttribute('ref');
if (m.getAttribute('role')==='right') rightId=m.getAttribute('ref');
}
const isCross = rsub==='crosswalk';
const group=isCross?crosswalkGroup:roadGroup, lineMat=isCross?crossLineMat:roadLineMat;
const leftRefs = wayMap.get(leftId) || [];
const rightRefs = wayMap.get(rightId) || [];
const leftPts = wayToVectors(nodeMap,wayMap,leftId).map(nodeToVec);
let rightPts = wayToVectors(nodeMap,wayMap,rightId).map(nodeToVec);
if (leftPts.length >= 2 && rightPts.length >= 2) {
const ldx = leftPts[leftPts.length-1].x - leftPts[0].x;
const ldz = leftPts[leftPts.length-1].z - leftPts[0].z;
const rdx = rightPts[rightPts.length-1].x - rightPts[0].x;
const rdz = rightPts[rightPts.length-1].z - rightPts[0].z;
if (ldx*rdx + ldz*rdz < 0) rightPts = [...rightPts].reverse();
}
const lineColor = isCross ? 0x666666 : 0x777777;
const lineOpacity = isCross ? 0.5 : 1;
const lL=buildLineMesh(leftPts,lineColor,lineOpacity), lR=buildLineMesh(rightPts,lineColor,lineOpacity);
if (lL) group.add(lL); if (lR) group.add(lR);
if (isCross) {
const stripes = buildCrosswalkStripes(leftPts, rightPts);
if (stripes) group.add(stripes);
} else {
const fillColor = rsub==='road' ? 0x2a2a36 : 0x28283a;
const fill = buildLaneletMesh(leftPts, rightPts, fillColor, 0, true, 0.02);
if (fill) group.add(fill);
}
isCross ? crossCount++ : roadCount++;
// 레인 중심점 계산
const allPts = [...leftPts, ...rightPts];
const cx = allPts.reduce((s,p)=>s+p.x,0)/allPts.length;
const cz = allPts.reduce((s,p)=>s+p.z,0)/allPts.length;
laneletList.push({ id:relId, leftRefs, rightRefs, leftPts, rightPts, cx, cz, isCross });
// 시작 노드 인덱스 등록 (이 레인의 시작점 → 레인 ID)
const lStart = leftRefs[0];
const rStart = rightRefs[0];
const key = `${lStart}_${rStart}`;
if (!endNodeIndex.has(key)) endNodeIndex.set(key, []);
endNodeIndex.get(key).push(relId);
}
// 연결 그래프 구축: 각 레인의 끝점 → 그 끝점에서 시작하는 다음 레인들
laneletGraph.clear();
for (const ll of laneletList) {
const lEnd = ll.leftRefs[ll.leftRefs.length-1];
const rEnd = ll.rightRefs[ll.rightRefs.length-1];
const key = `${lEnd}_${rEnd}`;
laneletGraph.set(ll.id, endNodeIndex.get(key) || []);
}
for (const [id,wt] of wayTypes) {
if (wt.type!=='stop_line') continue;
const mesh = buildStopLine(wayToVectors(nodeMap,wayMap,id).map(nodeToVec));
if (mesh) stoplineGroup.add(mesh);
}
for (const rel of doc.querySelectorAll('relation')) {
let rtype='',rsub='';
for (const tag of rel.querySelectorAll('tag')) {
if (tag.getAttribute('k')==='type') rtype=tag.getAttribute('v');
if (tag.getAttribute('k')==='subtype') rsub=tag.getAttribute('v');
}
if (rtype!=='regulatory_element'||rsub!=='traffic_light') continue;
tlCount++;
const tlRelId = rel.getAttribute('id');
const refersGroups = [], bulbGroups = [];
let refLinePts = null;
for (const m of rel.querySelectorAll('member')) {
if (m.getAttribute('type')!=='way') continue;
const role=m.getAttribute('role'), ref=m.getAttribute('ref');
const pts=wayToVectors(nodeMap,wayMap,ref).map(nodeToVec);
if (pts.length < 1) continue;
if (role==='refers') refersGroups.push(pts);
else if (role==='light_bulbs') bulbGroups.push(pts);
else if (role==='ref_line' && !refLinePts) refLinePts = pts;
}
const GY = refLinePts ? refLinePts.reduce((s,p)=>s+p.y,0)/refLinePts.length : -2.5;
const poleMat = new THREE.MeshBasicMaterial({color:0xaaaaaa,transparent:true,opacity:0.9});
const poleEdgeMat = new THREE.LineBasicMaterial({color:0x666666});
const armMat = new THREE.LineBasicMaterial({color:0x333333});
const bulbColors = [0xff1100, 0xffaa00, 0x00dd44];
for (const bulbPts of bulbGroups) {
if (!bulbPts.length) continue;
const cx=bulbPts.reduce((s,p)=>s+p.x,0)/bulbPts.length;
const cy=bulbPts.reduce((s,p)=>s+p.y,0)/bulbPts.length;
const cz=bulbPts.reduce((s,p)=>s+p.z,0)/bulbPts.length;
const ARM_LEN = 3.0;
let poleX=cx, poleZ=cz;
let bestRef=null, bestDist=Infinity;
for (const rp of refersGroups) {
const mx=rp.reduce((s,p)=>s+p.x,0)/rp.length;
const mz=rp.reduce((s,p)=>s+p.z,0)/rp.length;
const d2=(mx-cx)**2+(mz-cz)**2;
if (d2<bestDist) { bestDist=d2; bestRef=rp; }
}
if (bestRef && bestRef.length >= 2) {
const fdx=bestRef[bestRef.length-1].x-bestRef[0].x;
const fdz=bestRef[bestRef.length-1].z-bestRef[0].z;
const fLen=Math.sqrt(fdx*fdx+fdz*fdz)||1;
poleX = cx - (fdx/fLen)*ARM_LEN;
poleZ = cz - (fdz/fLen)*ARM_LEN;
}
const poleH = Math.max(cy-GY, 3.0)+0.8;
const poleGeom = new THREE.CylinderGeometry(0.08,0.11,poleH,6);
const pole = new THREE.Mesh(poleGeom, poleMat);
pole.add(new THREE.LineSegments(new THREE.EdgesGeometry(poleGeom,5), poleEdgeMat));
pole.position.set(poleX, GY+poleH/2, poleZ);
tlGroup.add(pole);
const armY = GY+poleH;
tlGroup.add(new THREE.Line(
new THREE.BufferGeometry().setFromPoints([
new THREE.Vector3(poleX, armY, poleZ),
new THREE.Vector3(cx, armY, cz),
]), armMat));
if (armY-cy > 0.2) {
tlGroup.add(new THREE.Line(
new THREE.BufferGeometry().setFromPoints([
new THREE.Vector3(cx, armY, cz),
new THREE.Vector3(cx, cy, cz),
]), armMat));
}
let yaw = 0;
if (refLinePts && refLinePts.length >= 2) {
const p0=refLinePts[0], p1=refLinePts[refLinePts.length-1];
const dx=p1.x-p0.x, dz=p1.z-p0.z;
yaw = Math.atan2(-dz, dx);
const lx=refLinePts.reduce((s,p)=>s+p.x,0)/refLinePts.length;
const lz=refLinePts.reduce((s,p)=>s+p.z,0)/refLinePts.length;
if (Math.sin(yaw)*(lx-cx)+Math.cos(yaw)*(lz-cz) < 0) yaw += Math.PI;
}
const hwGeom=new THREE.BoxGeometry(1.5, 0.5, 0.3);
const hw=new THREE.Mesh(hwGeom,
new THREE.MeshBasicMaterial({color:0x111122,transparent:true,opacity:0.92}));
hw.position.set(cx,cy,cz);
hw.rotation.y = yaw;
hw.userData.tlId = tlRelId;
hw.add(new THREE.LineSegments(new THREE.EdgesGeometry(hwGeom),
new THREE.LineBasicMaterial({color:0x446688})));
const bulbOffsets = [
new THREE.Vector3(-0.45, 0, 0.18),
new THREE.Vector3( 0.00, 0, 0.18),
new THREE.Vector3( 0.45, 0, 0.18),
];
for (let b=0;b<3;b++) {
const bulb=new THREE.Mesh(new THREE.SphereGeometry(0.13,8,6),
new THREE.MeshBasicMaterial({color:bulbColors[b]}));
bulb.position.copy(bulbOffsets[b]);
hw.add(bulb);
}
tlGroup.add(hw);
const existing = trafficLights.find(t => t.id === tlRelId);
const sharedState = existing ? existing.state : Math.floor(Math.random() * 3);
const sharedTimer = existing ? existing.timer : Math.random() * 5;
trafficLights.push({ mesh: hw, id: tlRelId, state: sharedState, timer: sharedTimer, duration: 5 });
}
}
// ── Parking spaces ──
const parkingGroup = new THREE.Group();
let parkingCount = 0;
for (const way of doc.querySelectorAll('way')) {
const tags = {};
for (const t of way.querySelectorAll('tag')) tags[t.getAttribute('k')] = t.getAttribute('v');
if (tags['type'] !== 'parking_space') continue;
const width = parseFloat(tags['width'] || '2.5');
const pts = Array.from(way.querySelectorAll('nd'))
.map(n => nodeMap.get(n.getAttribute('ref'))).filter(Boolean).map(nodeToVec);
if (pts.length < 2) continue;
const a = pts[0], b = pts[pts.length - 1];
const fill = buildParkingLotMesh([a, b], width);
if (fill) parkingGroup.add(fill);
parkingCount++;
}
scene.add(roadGroup); scene.add(crosswalkGroup); scene.add(stoplineGroup); scene.add(tlGroup);
scene.add(parkingGroup);
layerObjects.road=roadGroup; layerObjects.crosswalk=crosswalkGroup;
layerObjects.stopline=stoplineGroup; layerObjects.trafficlight=tlGroup;
layerObjects.parking=parkingGroup;
document.getElementById('s-road').textContent=roadCount;
document.getElementById('s-cross').textContent=crossCount;
setText('s-tl', tlCount);
document.getElementById('s-parking').textContent=parkingCount;
// Lanelet 기반 가상 환경 생성: PCD 없이도 도로 주변 배경을 구성합니다.
createFakeBuildings();
createStreetTrees();
setProgress(100, 'Done');
}
// ──────────────────────────────────────────────────────
// Procedural environment from Lanelet2 only (no PCD required)
// ──────────────────────────────────────────────────────
function seededRandom(n) {
const x = Math.sin(n * 127.1 + 311.7) * 43758.5453;
return x - Math.floor(x);
}
function seededRange(n, min, max) { return min + seededRandom(n) * (max - min); }
function distanceToLaneletEdges(x, z) {
let best = Infinity;
for (const ll of laneletList) {
for (const pts of [ll.leftPts, ll.rightPts]) {
for (let i = 0; i < pts.length - 1; i++) {
const ax = pts[i].x, az = pts[i].z;
const bx = pts[i + 1].x, bz = pts[i + 1].z;
const vx = bx - ax, vz = bz - az;
const len2 = vx * vx + vz * vz;
if (len2 < 0.01) continue;
const t = Math.max(0, Math.min(1, ((x - ax) * vx + (z - az) * vz) / len2));
const px = ax + t * vx, pz = az + t * vz;
const dx = x - px, dz = z - pz;
best = Math.min(best, Math.sqrt(dx * dx + dz * dz));
}
}
}
return best;
}
function createFakeBuildings() {
if (layerObjects.building) { scene.remove(layerObjects.building); layerObjects.building = null; }
if (!laneletList.length) return;
const group = new THREE.Group();
const roadPts = [];
for (const ll of laneletList) {
if (ll.isCross) continue;
for (const p of [...ll.leftPts, ...ll.rightPts]) roadPts.push(p);
}
if (!roadPts.length) return;
let minX = Infinity, maxX = -Infinity, minZ = Infinity, maxZ = -Infinity;
for (const p of roadPts) {
minX = Math.min(minX, p.x); maxX = Math.max(maxX, p.x);
minZ = Math.min(minZ, p.z); maxZ = Math.max(maxZ, p.z);
}
const margin = 55;
minX -= margin; maxX += margin; minZ -= margin; maxZ += margin;
const GROUND = 0;
const STEP = 32;
let seed = 1;
let count = 0;
for (let x = Math.floor(minX / STEP) * STEP; x <= maxX; x += STEP) {
for (let z = Math.floor(minZ / STEP) * STEP; z <= maxZ; z += STEP) {
seed++;
const bx = x + seededRange(seed * 7 + 1, -8, 8);
const bz = z + seededRange(seed * 7 + 2, -8, 8);
const d = distanceToLaneletEdges(bx, bz);
if (d < 12 || d > 55) continue;
const w = seededRange(seed * 7 + 3, 7, 18);
const depth = seededRange(seed * 7 + 4, 7, 18);
const h = seededRange(seed * 7 + 5, 4, 18);
const geom = new THREE.BoxGeometry(w, h, depth);
const body = new THREE.Mesh(geom, new THREE.MeshBasicMaterial({ color: 0x06101f, transparent: true, opacity: 0.84 }));
body.add(new THREE.LineSegments(new THREE.EdgesGeometry(geom, 5), new THREE.LineBasicMaterial({ color: h > 25 ? 0x2b6cff : 0x1a3a88, transparent: true, opacity: 0.9 })));
body.position.set(bx, GROUND + h / 2, bz);
group.add(body);
count++;
}
}
group.visible = layerState.building;
scene.add(group);
layerObjects.building = group;
const el = document.getElementById('s-buildings');
if (el) el.textContent = count.toLocaleString();
}
function createStreetTrees() {
if (layerObjects.tree) { scene.remove(layerObjects.tree); layerObjects.tree = null; }
if (!laneletList.length) return;
const group = new THREE.Group();
const GROUND = 0;
const SPACING = 20;
const OFFSET = 5.0;
const placed = new Set();
let treeCount = 0;
const trunkGeom = new THREE.CylinderGeometry(0.12, 0.18, 2.4, 6);
const leafGeom = new THREE.IcosahedronGeometry(1.45, 1);
const trunkMat = new THREE.MeshBasicMaterial({ color: 0x251103, transparent: true, opacity: 0.78 });
const leafMat = new THREE.MeshBasicMaterial({ color: 0x042010, transparent: true, opacity: 0.72 });
function placeTree(x, z, seed) {
const scale = seededRange(seed, 0.75, 1.25);
const g = new THREE.Group();
const trunk = new THREE.Mesh(trunkGeom, trunkMat);
trunk.add(new THREE.LineSegments(new THREE.EdgesGeometry(trunkGeom, 5), new THREE.LineBasicMaterial({ color: 0x6b461e })));
trunk.scale.setScalar(scale);
trunk.position.y = GROUND + 1.2 * scale;
g.add(trunk);
const leaf = new THREE.Mesh(leafGeom, leafMat);
leaf.add(new THREE.LineSegments(new THREE.EdgesGeometry(leafGeom, 5), new THREE.LineBasicMaterial({ color: 0x31d66a, transparent: true, opacity: 0.85 })));
leaf.scale.setScalar(scale);
leaf.position.y = GROUND + 3.1 * scale;
leaf.rotation.y = seededRange(seed + 2, 0, Math.PI);
g.add(leaf);
g.position.set(x, 0, z);
group.add(g);
}
for (const ll of laneletList) {
if (ll.isCross) continue;
const allPts = [...ll.leftPts, ...ll.rightPts];
const cx = allPts.reduce((s, p) => s + p.x, 0) / allPts.length;
const cz = allPts.reduce((s, p) => s + p.z, 0) / allPts.length;
for (const pts of [ll.leftPts, ll.rightPts]) {
for (let i = 0; i < pts.length - 1; i++) {
const p0 = pts[i], p1 = pts[i + 1];
const dx = p1.x - p0.x, dz = p1.z - p0.z;
const len = Math.sqrt(dx * dx + dz * dz);
if (len < 3 || len > 200) continue;
let nx = -dz / len, nz = dx / len;
const mx = (p0.x + p1.x) / 2, mz = (p0.z + p1.z) / 2;
if (nx * (cx - mx) + nz * (cz - mz) > 0) { nx = -nx; nz = -nz; }
const steps = Math.floor(len / SPACING);
for (let s = 0; s <= steps; s++) {
const t = (s + 0.5) / (steps + 1);
const tx = p0.x + dx * t + nx * OFFSET;
const tz = p0.z + dz * t + nz * OFFSET;
if (distanceToLaneletEdges(tx, tz) < 3) continue;
const key = `${Math.round(tx / 5)}_${Math.round(tz / 5)}`;
if (placed.has(key)) continue;
placed.add(key);
placeTree(tx, tz, treeCount * 11 + 3);
treeCount++;
}
}
}
}
group.visible = layerState.tree;
scene.add(group);
layerObjects.tree = group;
const el = document.getElementById('s-trees');
if (el) el.textContent = treeCount.toLocaleString();
}
// ──────────────────────────────────────────────────────
// ── 레인 그래프 전역 데이터 ──
const laneletList = []; // {id, leftRefs, rightRefs, leftPts, rightPts, cx, cz, isCross}
const laneletGraph = new Map(); // laneletId → successorIds[]
let pathHighlight = null; // 경로 하이라이트 Group
function findNearestLanelet(pos, heading) {
const headingRad = (heading ?? 0) * Math.PI / 180;
const fwdX = Math.sin(headingRad);
const fwdZ = -Math.cos(headingRad);
let best = null, bestDist = Infinity;
for (const ll of laneletList) {
if (ll.isCross) continue;
// 레인 방향 벡터 (시작→끝)
const sL = ll.leftPts[0], sR = ll.rightPts[0];
const eL = ll.leftPts[ll.leftPts.length-1], eR = ll.rightPts[ll.rightPts.length-1];
const lDx = (eL.x+eR.x) - (sL.x+sR.x);
const lDz = (eL.z+eR.z) - (sL.z+sR.z);
const lLen = Math.sqrt(lDx*lDx + lDz*lDz) || 1;
// 진행 방향과 역방향 레인 제외
if (heading != null && (fwdX*(lDx/lLen) + fwdZ*(lDz/lLen)) < 0) continue;
const dx = ll.cx - pos.x, dz = ll.cz - pos.z;
const d = dx*dx + dz*dz;
if (d < bestDist) { bestDist = d; best = ll.id; }
}
return best;
}
function bfsPath(startId, goalId) {
if (startId === goalId) return [startId];
const visited = new Set([startId]);
const queue = [[startId]];
while (queue.length) {
const path = queue.shift();
const cur = path[path.length-1];
for (const next of (laneletGraph.get(cur) || [])) {
if (visited.has(next)) continue;
const newPath = [...path, next];
if (next === goalId) return newPath;
visited.add(next);
queue.push(newPath);
}
}
return null;
}
function showPathHighlight(laneletIds) {
if (pathHighlight) { scene.remove(pathHighlight); pathHighlight = null; }
if (!laneletIds || laneletIds.length === 0) return;
const group = new THREE.Group();
const mat = new THREE.MeshBasicMaterial({
color: 0x00ff66, side: THREE.DoubleSide, transparent: true, opacity: 0.45, depthWrite: false
});
const idSet = new Set(laneletIds);
for (const ll of laneletList) {
if (!idSet.has(ll.id)) continue;
const mesh = buildLaneletMesh(ll.leftPts, ll.rightPts, 0x00ff66);
if (mesh) { mesh.material = mat.clone(); group.add(mesh); }
}
scene.add(group);
pathHighlight = group;
}
function updateRouteHighlight() {
if (!poseState.initial || !poseState.goal) return;
const startId = findNearestLanelet(poseState.initial.pos, poseState.initial.heading);
const goalId = findNearestLanelet(poseState.goal.pos, poseState.goal.heading);
if (!startId || !goalId) return;
const path = bfsPath(startId, goalId);
showPathHighlight(path);
}
// Pose system
// ──────────────────────────────────────────────────────
let roadElevation = 0; // loadLanelet2 후 eleMin 값으로 업데이트됨
const groundPlane = new THREE.Plane(new THREE.Vector3(0,1,0), 0);
const raycaster = new THREE.Raycaster();
const mouseVec = new THREE.Vector2();
let poseMode = null; // null | 'initial' | 'goal'
let poseDragging = false;
let poseStartPoint = null;
const poseState = { initial: null, goal: null };
let vehicleMesh=null, goalMesh=null, tempArrow=null, polarGrid=null;
let _vehicleGroupCache = null; // 미리 처리된 차량 Group (clone용)
// GLB 프리로드 + EdgesGeometry 사전 계산
const _gltfLoader = new GLTFLoader();
const _dracoLoader = new DRACOLoader();
_dracoLoader.setDecoderPath('/draco/');
_gltfLoader.setDRACOLoader(_dracoLoader);
const animClock = new THREE.Clock();
async function loadVehicleGlb() {
if (ROBOT_STACK === 'nav2') {
_vehicleGroupCache = null;
return;
}
setProgress(90, 'Loading vehicle model...');
return new Promise((resolve) => {
_gltfLoader.load('/autoware-map/vehicle.glb', (gltf) => {
const model = gltf.scene;
model.rotation.y = - Math.PI / 2;
const box = new THREE.Box3().setFromObject(model);
const size = new THREE.Vector3();
box.getSize(size);
const scale = 4.5 / Math.max(size.x, size.z);
model.scale.setScalar(scale);
const box2 = new THREE.Box3().setFromObject(model);
model.position.y = -box2.min.y - 0.8;
model.traverse(o => {
if (o.isMesh) {
o.material.transparent = true;
o.material.opacity = 0.35;
o.material.depthWrite = false;
}
});
_vehicleGroupCache = model;
resolve();
}, undefined, (err) => {
console.warn('vehicle.glb 로드 실패, 박스 모델 사용:', err);
resolve(); // 실패해도 로딩 계속 진행
});
});
}
let plannedLine=null, actualLine=null; // 백엔드 trajectory 라인
let plannedPts=[]; // 전체 계획 경로 보관
let routeLaneletBorder=null; // 계획 경로가 지나가는 lanelet boundary 강조
let routeUpdateFrame=0; // 남은 경로 갱신 프레임 제한
const PLANNED_TRAIL_M = 6.0; // 차량 뒤쪽에 남겨둘 주행 예정선 길이(m)
const ACTUAL_TRAIL_M = 3.0; // 주황 실제 주행선은 예정선보다 약 3m 짧게 유지
let actualPts=[], actualAnimIdx=0, actualAnimTimer=null;
let driveWs = null;
let stateWs = null;
let driveStartTime = null;
const DRIVE_ARRIVAL_M = 3.0;
const WS_BRIDGE = ROS_BRIDGE_WS;
const VIEWER_STATE_KEY = `autoware-viewer:state:${MAP_SOURCE}:${ROS_BRIDGE_WS}`;
function serializePose(pose) {
if (!pose?.pos) return null;
return {
pos: { x: pose.pos.x, y: pose.pos.y, z: pose.pos.z },
heading: pose.heading,
};
}
function saveViewerState(extra = {}) {
try {
const previous = readViewerState();
const state = {
initial: serializePose(poseState.initial),
goal: serializePose(poseState.goal),
planned: ROBOT_STACK === 'nav2' ? [] : plannedPts || [],
lastVehicle: previous?.lastVehicle || null,
driving: Boolean(driveWs),
driveCompleted: Boolean(previous?.driveCompleted),
updatedAt: Date.now(),
...extra,
};
window.sessionStorage.setItem(VIEWER_STATE_KEY, JSON.stringify(state));
} catch {}
}
function readViewerState() {
try {
return JSON.parse(window.sessionStorage.getItem(VIEWER_STATE_KEY) || 'null');
} catch {
return null;
}
}
// ── Polar Grid ──
let polarPulse = null;
const PULSE_MAX_R = 90;
function buildCircle(r, y, color, opacity) {
const divs = 64, pts = [];
for (let i = 0; i <= divs; i++) {
const a = (i / divs) * Math.PI * 2;
pts.push(new THREE.Vector3(Math.sin(a)*r, y, Math.cos(a)*r));
}
return new THREE.Line(
new THREE.BufferGeometry().setFromPoints(pts),
new THREE.LineBasicMaterial({ color, transparent: true, opacity })
);
}
function createPolarGrid(pos) {
if (ROBOT_STACK === 'nav2') return;
if (polarGrid) { scene.remove(polarGrid); polarGrid = null; }
polarPulse = null;
const g = new THREE.Group();
[10, 20, 30, 40, 50, 60, 70, 80, 90].forEach((r, i) => {
g.add(buildCircle(r, 0.03, 0xffffff, 0.35 - i * 0.025));
});
const pulseLine = buildCircle(0.1, 0.04, 0xffffff, 0.9);
g.add(pulseLine);
polarPulse = { line: pulseLine, r: 0 };
g.position.set(pos.x, roadElevation + 0.03, pos.z);
g.visible = polarGridVisible;
scene.add(g);
polarGrid = g;
}
function updatePolarPulse() {
if (!polarPulse || !polarGrid || !polarGrid.visible) return;
polarPulse.r += 1.4;
if (polarPulse.r > PULSE_MAX_R) polarPulse.r = 0;
const r = polarPulse.r, t = r / PULSE_MAX_R;
const pts = [];
for (let i = 0; i <= 64; i++) {
const a = (i / 64) * Math.PI * 2;
pts.push(new THREE.Vector3(Math.sin(a)*r, 0.2, Math.cos(a)*r));
}
polarPulse.line.geometry.setFromPoints(pts);
polarPulse.line.material.opacity = 1.0 - t;
}
// ── Vehicle mesh ──
function createVehicleMesh() {
const g = new THREE.Group();
if (ROBOT_STACK === 'nav2') {
const length = Math.max(0.2, NAV2_VEHICLE_LENGTH);
const width = Math.max(0.2, NAV2_VEHICLE_WIDTH);
const height = Math.max(0.05, NAV2_VEHICLE_HEIGHT);
const bodyG = new THREE.BoxGeometry(width, height, length);
const body = new THREE.Mesh(
bodyG,
new THREE.MeshBasicMaterial({ color: 0x20d46b, transparent: true, opacity: 0.55, depthWrite: false })
);
body.position.y = height * 0.5;
g.add(body);
const edge = new THREE.LineSegments(
new THREE.EdgesGeometry(bodyG),
new THREE.LineBasicMaterial({ color: 0xd8ffe7, transparent: true, opacity: 0.9 })
);
edge.position.copy(body.position);
g.add(edge);
const nose = new THREE.Mesh(
new THREE.ConeGeometry(Math.max(width * 0.28, 0.08), Math.max(length * 0.28, 0.16), 3),
new THREE.MeshBasicMaterial({ color: 0xffffff, transparent: true, opacity: 0.85 })
);
nose.rotation.x = Math.PI / 2;
nose.position.set(0, height + 0.03, -length * 0.5);
g.add(nose);
} else if (_vehicleGroupCache) {
// 사전 처리된 모델 clone (EdgesGeometry 재계산 없음 → 멈춤 없음)
g.add(_vehicleGroupCache.clone(true));
} else {
// GLB 없을 때 폴백: 박스
const bodyG = new THREE.BoxGeometry(2.1, 1.6, 4.6);
g.add(new THREE.Mesh(bodyG, new THREE.MeshBasicMaterial({ color:0x0055aa, transparent:true, opacity:0.70 })));
g.add(new THREE.Mesh(bodyG, new THREE.MeshBasicMaterial({ color:0x44aaff, wireframe:true })));
}
return g;
}
// ── Goal mesh (thick axes) ──
function createGoalMesh() {
if (ROBOT_STACK === 'nav2') return null;
const group = new THREE.Group();
const makeAxis = (color, dir, length = 1.8, radius = 0.14) => {
const mat = new THREE.MeshBasicMaterial({ color });
const cyl = new THREE.Mesh(
new THREE.CylinderGeometry(radius, radius, length, 16),
mat
);
// 중심 기준으로 만들기
cyl.position.y = length / 2;
const g = new THREE.Group();
g.add(cyl);
// 방향 정렬
const q = new THREE.Quaternion();
q.setFromUnitVectors(new THREE.Vector3(0, 1, 0), dir.clone().normalize());
g.quaternion.copy(q);
return g;
};
group.add(makeAxis(0xff2222, new THREE.Vector3(0, 0, -1), 1.4, 0.18));
group.add(makeAxis(0x22ff44, new THREE.Vector3(-1, 0, 0), 1.1, 0.14));
group.add(makeAxis(0x2266ff, new THREE.Vector3(0, 1, 0), 1.2, 0.14));
// 중심점
const base = new THREE.Mesh(
new THREE.SphereGeometry(0.22, 16, 12),
new THREE.MeshBasicMaterial({ color: 0xffffff })
);
group.add(base);
return group;
}
// ── Coord helpers ──
function threeToLocal(v) { return { x: v.x + CENTER_X, y: -v.z + CENTER_Y }; }
function calcHeading(dx, dz) {
let deg = Math.atan2(dx, -dz) * 180 / Math.PI;
return ((deg % 360) + 360) % 360;
}
// 차량 pose는 rear axle/base_link 기준으로 유지하고,
// 화면에 보이는 GLB만 차량 중심 위치로 앞으로 보정합니다.
const REAR_AXLE_TO_CENTER = ROBOT_STACK === 'nav2' ? NAV2_BASE_TO_CENTER : 1.4; // m: 후륜축(base_link) → 차량 중심 보정값
function rearAxleToVehicleCenter(pos, headingDeg) {
const yaw = headingDeg * Math.PI / 180;
return {
x: pos.x + Math.sin(yaw) * REAR_AXLE_TO_CENTER,
y: pos.y,
z: pos.z - Math.cos(yaw) * REAR_AXLE_TO_CENTER,
};
}
function rearAxleToVehicleCenterRad(x, z, yawRad) {
return {
x: x + Math.sin(yawRad) * REAR_AXLE_TO_CENTER,
z: z - Math.cos(yawRad) * REAR_AXLE_TO_CENTER,
};
}
function setVehicleVisualPoseFromRearAxle(x, z, yawRad, y = roadElevation + 0.8) {
if (!vehicleMesh) {
vehicleMesh = createVehicleMesh();
scene.add(vehicleMesh);
}
const center = rearAxleToVehicleCenterRad(x, z, yawRad);
vehicleMesh.position.set(center.x, y, center.z);
vehicleMesh.rotation.y = -yawRad;
}
function getGroundPoint(e) {
const rect = renderer.domElement.getBoundingClientRect();
mouseVec.x = (e.clientX - rect.left) / rect.width * 2 - 1;
mouseVec.y = -(e.clientY - rect.top) / rect.height * 2 + 1;
raycaster.setFromCamera(mouseVec, camera);
const hit = new THREE.Vector3();
return raycaster.ray.intersectPlane(groundPlane, hit) ? hit : null;
}
// ── Temp drag arrow ──
function updateTempArrow(from, to) {
if (tempArrow) { scene.remove(tempArrow); tempArrow = null; }
const dx = to.x - from.x, dz = to.z - from.z;
const len = Math.sqrt(dx*dx + dz*dz);
if (len < 0.3) return;
const MAX_ARROW = ROBOT_STACK === 'nav2' ? 1 : 2;
const dir = new THREE.Vector3(dx/len, 0, dz/len);
const origin = from.clone(); origin.y += 0.5;
const color = poseMode === 'initial' ? 0x00ccff : 0xff7700;
const arrowLen = Math.min(Math.max(len, MAX_ARROW), MAX_ARROW);
tempArrow = new THREE.ArrowHelper(dir, origin, arrowLen, color, 0, 0);
scene.add(tempArrow);
}
// ── Apply confirmed pose ──
function applyPose(type, pos, heading, options = {}) {
const local = threeToLocal(pos);
poseState[type] = { pos: pos.clone(), heading };
if (type === 'initial') {
if (driveWs) { driveWs.close(); driveWs = null; fetch(`${ROS_BRIDGE_HTTP}/disengage`, { method: 'POST' }).catch(() => {}); }
const driveBtn = document.getElementById('btn-drive');
const label = driveBtn.querySelector('.pose-label');
const icon = driveBtn.querySelector('.pose-icon');
label.textContent = 'Start';
icon.innerHTML = '&#9654;';
driveBtn.onclick = window.startDrive;
driveBtn.disabled = true;
if (goalMesh) { scene.remove(goalMesh); goalMesh = null; }
if (plannedLine) { scene.remove(plannedLine); plannedLine = null; }
if (actualLine) { scene.remove(actualLine); actualLine = null; }
if (pathHighlight) { scene.remove(pathHighlight); pathHighlight = null; }
if (routeLaneletBorder) { scene.remove(routeLaneletBorder); routeLaneletBorder = null; }
actualPts = [];
poseState.goal = null;
document.getElementById('pi-goal-sec').style.display = 'none';
document.getElementById('pi-dist-sec').style.display = 'none';
document.getElementById('pi-mid-div').style.display = 'none';
setPlanStatus('');
if (vehicleMesh) scene.remove(vehicleMesh);
vehicleMesh = createVehicleMesh();
const vehicleCenter = rearAxleToVehicleCenter(pos, heading);
vehicleMesh.position.set(vehicleCenter.x, roadElevation + 0.8, vehicleCenter.z);
vehicleMesh.rotation.y = -(heading * Math.PI / 180);
scene.add(vehicleMesh);
if (ROBOT_STACK !== 'nav2') createPolarGrid({ x: vehicleCenter.x, z: vehicleCenter.z });
document.getElementById('pi-initial-sec').style.display = 'block';
document.getElementById('pi-ix').textContent = local.x.toFixed(2);
document.getElementById('pi-iy').textContent = local.y.toFixed(2);
document.getElementById('pi-ih').textContent = heading.toFixed(1) + '°';
} else {
if (goalMesh) scene.remove(goalMesh);
goalMesh = createGoalMesh();
if (goalMesh) {
goalMesh.position.set(pos.x, roadElevation + 0.16, pos.z);
goalMesh.rotation.y = -(heading * Math.PI / 180);
scene.add(goalMesh);
}
document.getElementById('pi-goal-sec').style.display = 'block';
document.getElementById('pi-gx').textContent = local.x.toFixed(2);
document.getElementById('pi-gy').textContent = local.y.toFixed(2);
document.getElementById('pi-gh').textContent = heading.toFixed(1) + '°';
}
const isSmall = window.innerWidth < 700 || window.innerHeight < 500;
if (!isSmall) {
document.getElementById('pose-info-wrap').style.display = 'block';
}
// Distance between poses
if (poseState.initial && poseState.goal) {
const dx = poseState.initial.pos.x - poseState.goal.pos.x;
const dz = poseState.initial.pos.z - poseState.goal.pos.z;
document.getElementById('pi-dist-sec').style.display = 'block';
document.getElementById('pi-dist').textContent = Math.sqrt(dx*dx+dz*dz).toFixed(1) + ' m';
document.getElementById('pi-mid-div').style.display = 'block';
}
saveViewerState();
if (!options.skipPlan && poseState.initial && poseState.goal) requestPlan();
}
// ── Mode toggle ──
window.setPoseMode = function(mode) {
if (poseMode === mode) {
exitPoseMode(); return;
}
poseMode = mode;
controls.enabled = false;
document.body.classList.add('mode-pose');
document.querySelectorAll('.pose-btn').forEach(b => b.classList.remove('active'));
document.getElementById(`btn-${mode}-pose`).classList.add('active');
document.getElementById('pose-hint')?.classList.add('visible');
};
function exitPoseMode() {
poseMode = null;
controls.enabled = true;
document.body.classList.remove('mode-pose');
document.querySelectorAll('.pose-btn').forEach(b => b.classList.remove('active'));
document.getElementById('pose-hint')?.classList.remove('visible');
}
window.clearPoses = function() {
planGeneration++;
if (ROBOT_STACK === 'nav2') {
fetch(`${ROS_BRIDGE_HTTP}/clear_route`, { method: 'POST' }).catch(() => {});
} else {
fetch(`${ROS_BRIDGE_HTTP}/clear_route`, { method: 'POST' }).catch(() => {});
}
if (vehicleMesh) { scene.remove(vehicleMesh); vehicleMesh = null; }
if (goalMesh) { scene.remove(goalMesh); goalMesh = null; }
if (pathHighlight) { scene.remove(pathHighlight); pathHighlight = null; }
if (plannedLine) { scene.remove(plannedLine); plannedLine = null; }
if (routeLaneletBorder) { scene.remove(routeLaneletBorder); routeLaneletBorder = null; }
plannedPts = [];
routeUpdateFrame = 0;
if (actualLine) { scene.remove(actualLine); actualLine = null; }
if (actualAnimTimer) { clearInterval(actualAnimTimer); actualAnimTimer = null; }
actualPts = []; actualAnimIdx = 0;
document.getElementById('btn-drive').disabled = true;
setPlanStatus('');
if (tempArrow) { scene.remove(tempArrow); tempArrow = null; }
if (polarGrid) { scene.remove(polarGrid); polarGrid = null; }
poseState.initial = null; poseState.goal = null;
window.sessionStorage.removeItem(VIEWER_STATE_KEY);
document.getElementById('pi-initial-sec').style.display = 'none';
document.getElementById('pi-goal-sec').style.display = 'none';
document.getElementById('pi-dist-sec').style.display = 'none';
document.getElementById('pi-mid-div').style.display = 'none';
document.getElementById('pose-info-wrap').style.display = 'none';
document.getElementById('pose-info').style.display = 'none';
poseInfoExpanded = false;
document.getElementById('pose-info-toggle').style.color = '#445';
document.getElementById('pose-info-toggle').style.background = 'rgba(10,10,30,0.88)';
};
// ── Backend plan / drive ──
function setPlanStatus(msg) {
const el = document.getElementById('plan-status');
if (!msg) { el.style.display = 'none'; return; }
el.textContent = msg;
el.style.display = 'block';
}
function drawTrajectoryLine(pts, color, existingLine, smooth = true) {
if (existingLine) scene.remove(existingLine);
if (!pts || pts.length < 2) return null;
// pts format: [THREE.x, THREE.y, THREE.z, t]
// smooth=true: 계획경로(스플라인), smooth=false: 실시간 트레일(직선)
let corrected;
const trailY = roadElevation + 0.08;
if (smooth && pts.length >= 3) {
const ctrlPts = pts.map(p => new THREE.Vector3(p[0], trailY, p[2]));
const curve = new THREE.CatmullRomCurve3(ctrlPts);
corrected = curve.getPoints(Math.max(pts.length * 4, 60));
} else {
corrected = pts.map(p => new THREE.Vector3(p[0], trailY, p[2]));
}
const HALF_W = 1; // 차선 안쪽에 들어오도록 (총 2.0m)
const verts = [];
const indices = [];
for (let i = 0; i < corrected.length; i++) {
const cur = corrected[i];
// 진행 방향 벡터
const fwd = i < corrected.length - 1
? new THREE.Vector3().subVectors(corrected[i + 1], cur).normalize()
: new THREE.Vector3().subVectors(cur, corrected[i - 1]).normalize();
// xz 평면 90° 회전 → 좌우 방향
const right = new THREE.Vector3(fwd.z, 0, -fwd.x);
verts.push(
cur.x - right.x * HALF_W, cur.y, cur.z - right.z * HALF_W, // 왼쪽
cur.x + right.x * HALF_W, cur.y, cur.z + right.z * HALF_W // 오른쪽
);
if (i > 0) {
const b = (i - 1) * 2;
indices.push(b, b + 1, b + 2, b + 1, b + 3, b + 2);
}
}
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(new Float32Array(verts), 3));
geom.setIndex(indices);
const mat = new THREE.MeshBasicMaterial({
color, side: THREE.DoubleSide,
transparent: true, opacity: 0.55,
depthTest: false, depthWrite: false,
});
const mesh = new THREE.Mesh(geom, mat);
mesh.renderOrder = 999;
scene.add(mesh);
return mesh;
}
function calcPathDistanceTable(pts) {
const acc = [0];
if (!pts || pts.length < 2) return acc;
for (let i = 1; i < pts.length; i++) {
const dx = pts[i][0] - pts[i - 1][0];
const dz = pts[i][2] - pts[i - 1][2];
acc.push(acc[i - 1] + Math.sqrt(dx * dx + dz * dz));
}
return acc;
}
function mapPointToThreeRoutePoint(point, center = getPayloadCenter(rosVectorPayload)) {
return [
Number(point.x) - center.x,
Number(point.z) || 0,
-(Number(point.y) - center.y),
Number(point.t) || 0,
];
}
function sampleRoutePoints(points, minDist = 0.5) {
if (!points || !points.length) return [];
const result = [points[0]];
let lastX = points[0][0];
let lastZ = points[0][2];
for (const point of points.slice(1)) {
const dx = point[0] - lastX;
const dz = point[2] - lastZ;
if (Math.sqrt(dx * dx + dz * dz) >= minDist) {
result.push(point.map((value, index) => index <= 3 ? Number(value.toFixed(3)) : value));
lastX = point[0];
lastZ = point[2];
}
}
return result;
}
function trimRouteBehindStart(points, start) {
if (!points || !points.length || !start) return points || [];
const headingRad = (start.heading * Math.PI) / 180;
const fwdX = Math.sin(headingRad);
const fwdZ = -Math.cos(headingRad);
return points.filter((point) => {
const dx = point[0] - start.pos.x;
const dz = point[2] - start.pos.z;
return dx * fwdX + dz * fwdZ >= -0.5;
});
}
async function requestAutowarePlanFromBridge() {
if (poseState.initial) {
const initRes = await fetch(`${ROS_BRIDGE_HTTP}/set_initial_pose`, {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify(threePoseToRosPayload(poseState.initial)),
});
if (!initRes.ok) throw new Error(await initRes.text());
await new Promise((resolve) => setTimeout(resolve, 800));
}
const routeRes = await fetch(`${ROS_BRIDGE_HTTP}/plan`, {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify(threePoseToRosPayload(poseState.goal)),
});
if (!routeRes.ok) throw new Error(await routeRes.text());
const center = getPayloadCenter(rosVectorPayload);
const planData = await routeRes.json();
const pathPoints = (planData.planned || []).map((point) => mapPointToThreeRoutePoint(point, center));
const trajectoryPoints = (planData.plannedTrajectory || []).map((point) => mapPointToThreeRoutePoint(point, center));
const visibleTrajectoryPoints = trimRouteBehindStart(trajectoryPoints, poseState.initial);
return {
planned: sampleRoutePoints(pathPoints, 0.5),
plannedTrajectory: sampleRoutePoints(visibleTrajectoryPoints, 0.5),
};
}
function findNearestPathIndex(pts, x, z) {
if (!pts || pts.length < 2) return 0;
let bestIdx = 0;
let bestD = Infinity;
for (let i = 0; i < pts.length; i++) {
const dx = pts[i][0] - x;
const dz = pts[i][2] - z;
const d = dx * dx + dz * dz;
if (d < bestD) {
bestD = d;
bestIdx = i;
}
}
return bestIdx;
}
function slicePathWithTrail(pts, x, z, trailM = PLANNED_TRAIL_M) {
if (!pts || pts.length < 2) return pts || [];
const nearestIdx = findNearestPathIndex(pts, x, z);
const distTable = calcPathDistanceTable(pts);
const targetDist = Math.max(0, distTable[nearestIdx] - trailM);
let startIdx = 0;
while (startIdx < distTable.length - 1 && distTable[startIdx + 1] < targetDist) startIdx++;
return pts.slice(startIdx);
}
function distancePointToPolyline2D(x, z, linePts) {
let best = Infinity;
if (!linePts || linePts.length < 2) return best;
for (let i = 0; i < linePts.length - 1; i++) {
const a = linePts[i];
const b = linePts[i + 1];
const vx = b.x - a.x;
const vz = b.z - a.z;
const len2 = vx * vx + vz * vz;
if (len2 < 1e-6) continue;
const t = Math.max(0, Math.min(1, ((x - a.x) * vx + (z - a.z) * vz) / len2));
const px = a.x + vx * t;
const pz = a.z + vz * t;
const dx = x - px;
const dz = z - pz;
best = Math.min(best, Math.sqrt(dx * dx + dz * dz));
}
return best;
}
function getLaneletCenterPts(ll) {
const n = Math.min(ll.leftPts.length, ll.rightPts.length);
const pts = [];
for (let i = 0; i < n; i++) {
pts.push(new THREE.Vector3(
(ll.leftPts[i].x + ll.rightPts[i].x) * 0.5,
(ll.leftPts[i].y + ll.rightPts[i].y) * 0.5,
(ll.leftPts[i].z + ll.rightPts[i].z) * 0.5
));
}
return pts;
}
function nearestRouteInfo2D(x, z, routePts) {
let best = { dist: Infinity, idx: 0, dirX: 0, dirZ: -1, px: 0, pz: 0 };
if (!routePts || routePts.length < 2) return best;
for (let i = 0; i < routePts.length - 1; i++) {
const ax = routePts[i][0];
const az = routePts[i][2];
const bx = routePts[i + 1][0];
const bz = routePts[i + 1][2];
const vx = bx - ax;
const vz = bz - az;
const len2 = vx * vx + vz * vz;
if (len2 < 1e-6) continue;
const t = Math.max(0, Math.min(1, ((x - ax) * vx + (z - az) * vz) / len2));
const px = ax + vx * t;
const pz = az + vz * t;
const dx = x - px;
const dz = z - pz;
const dist = Math.sqrt(dx * dx + dz * dz);
if (dist < best.dist) {
const len = Math.sqrt(len2);
best = {
dist,
idx: i,
dirX: vx / len,
dirZ: vz / len,
px,
pz,
};
}
}
return best;
}
function laneletDirectionNearPoint(ll, x, z) {
const centerPts = getLaneletCenterPts(ll);
let best = { dist: Infinity, dirX: 0, dirZ: -1 };
for (let i = 0; i < centerPts.length - 1; i++) {
const a = centerPts[i];
const b = centerPts[i + 1];
const vx = b.x - a.x;
const vz = b.z - a.z;
const len2 = vx * vx + vz * vz;
if (len2 < 1e-6) continue;
const t = Math.max(0, Math.min(1, ((x - a.x) * vx + (z - a.z) * vz) / len2));
const px = a.x + vx * t;
const pz = a.z + vz * t;
const dx = x - px;
const dz = z - pz;
const dist = Math.sqrt(dx * dx + dz * dz);
if (dist < best.dist) {
const len = Math.sqrt(len2);
best = { dist, dirX: vx / len, dirZ: vz / len };
}
}
return best;
}
function buildActiveLaneletByRouteIndex(routePts) {
const active = new Map();
if (!routePts || routePts.length < 2) return active;
const step = Math.max(1, Math.floor(routePts.length / 120));
const CENTER_MAX_DIST = 2.0;
const MIN_DIR_DOT = 0.55;
for (let i = 0; i < routePts.length; i += step) {
const p = routePts[i];
const i0 = Math.max(0, i - 1);
const i1 = Math.min(routePts.length - 1, i + 1);
const rdx = routePts[i1][0] - routePts[i0][0];
const rdz = routePts[i1][2] - routePts[i0][2];
const rlen = Math.sqrt(rdx * rdx + rdz * rdz);
if (rlen < 1e-6) continue;
const rux = rdx / rlen;
const ruz = rdz / rlen;
let bestLanelet = null;
let bestScore = Infinity;
for (const ll of laneletList) {
if (ll.isCross) continue;
const info = laneletDirectionNearPoint(ll, p[0], p[2]);
if (!Number.isFinite(info.dist)) continue;
if (info.dist > CENTER_MAX_DIST) continue;
const dot = info.dirX * rux + info.dirZ * ruz;
if (dot < MIN_DIR_DOT) continue;
const score = info.dist + (1 - dot) * 3.0;
if (score < bestScore) {
bestScore = score;
bestLanelet = ll;
}
}
if (bestLanelet) active.set(i, bestLanelet);
}
return active;
}
function nearestActiveLaneletId(routeIdx, activeByIndex) {
let bestId = null;
let bestDelta = Infinity;
for (const [idx, ll] of activeByIndex.entries()) {
const delta = Math.abs(idx - routeIdx);
if (delta < bestDelta) {
bestDelta = delta;
bestId = ll.id;
}
}
return bestId;
}
function polylineSegmentsNearRoute(linePts, routePts, activeByIndex, laneletId, maxDist = 2.4) {
const out = [];
if (!linePts || linePts.length < 2 || !routePts || routePts.length < 2) return out;
const MIN_SEG_DIR_DOT = 0.50;
for (let i = 0; i < linePts.length - 1; i++) {
const a = linePts[i];
const b = linePts[i + 1];
const mx = (a.x + b.x) * 0.5;
const mz = (a.z + b.z) * 0.5;
const routeInfo = nearestRouteInfo2D(mx, mz, routePts);
if (routeInfo.dist > maxDist) continue;
const activeId = nearestActiveLaneletId(routeInfo.idx, activeByIndex);
if (activeId !== laneletId) continue;
const sx = b.x - a.x;
const sz = b.z - a.z;
const slen = Math.sqrt(sx * sx + sz * sz);
if (slen < 1e-6 || slen > MAX_SEGMENT) continue;
const dot = Math.abs((sx / slen) * routeInfo.dirX + (sz / slen) * routeInfo.dirZ);
if (dot < MIN_SEG_DIR_DOT) continue;
out.push([a, b]);
}
return out;
}
function distancePointToRouteArray2D(x, z, routePts) {
return nearestRouteInfo2D(x, z, routePts).dist;
}
function buildSegmentLineMesh(segments, color = 0x00ccff, opacity = 0.98, yLift = 0.16, halfW = 0.08) {
if (!segments || !segments.length) return null;
const verts = [];
for (const [a, b] of segments) {
const dx = b.x - a.x;
const dz = b.z - a.z;
const len = Math.sqrt(dx * dx + dz * dz);
if (len < 1e-6 || len > MAX_SEGMENT) continue;
const rx = -dz / len * halfW;
const rz = dx / len * halfW;
const ya = a.y + yLift;
const yb = b.y + yLift;
verts.push(
a.x - rx, ya, a.z - rz, a.x + rx, ya, a.z + rz, b.x - rx, yb, b.z - rz,
b.x - rx, yb, b.z - rz, a.x + rx, ya, a.z + rz, b.x + rx, yb, b.z + rz
);
}
if (!verts.length) return null;
const geom = new THREE.BufferGeometry();
geom.setAttribute('position', new THREE.BufferAttribute(new Float32Array(verts), 3));
const mesh = new THREE.Mesh(geom, new THREE.MeshBasicMaterial({
color,
side: THREE.DoubleSide,
transparent: opacity < 1,
opacity,
depthTest: false,
depthWrite: false,
polygonOffset: true,
polygonOffsetFactor: -2,
polygonOffsetUnits: -2,
}));
mesh.renderOrder = 998;
return mesh;
}
function drawRouteLaneletBorders(routePts, old = null) {
if (old) scene.remove(old);
if (!routePts || routePts.length < 2) return null;
const group = new THREE.Group();
// route 각 구간에서 실제 진행 lanelet 1개만 고릅니다.
// 거리만 보지 않고 방향까지 같이 보므로 옆 차선/교차 차선이 같이 칠해지는 현상을 막습니다.
const activeByIndex = buildActiveLaneletByRouteIndex(routePts);
const selected = new Map();
for (const ll of activeByIndex.values()) selected.set(ll.id, ll);
const BOUNDARY_NEAR_ROUTE_M = 2.4;
for (const ll of selected.values()) {
const leftSegments = polylineSegmentsNearRoute(ll.leftPts, routePts, activeByIndex, ll.id, BOUNDARY_NEAR_ROUTE_M);
const rightSegments = polylineSegmentsNearRoute(ll.rightPts, routePts, activeByIndex, ll.id, BOUNDARY_NEAR_ROUTE_M);
const left = buildSegmentLineMesh(leftSegments, 0x00ccff, 0.98, 0.16, 0.08);
const right = buildSegmentLineMesh(rightSegments, 0x00ccff, 0.98, 0.16, 0.08);
if (left) group.add(left);
if (right) group.add(right);
const fill = buildLaneletMesh(ll.leftPts, ll.rightPts, 0x00ff66, 0.13, false, 0.05);
if (fill) group.add(fill);
}
if (!group.children.length) return null;
scene.add(group);
return group;
}
function updateRemainingRouteByVehicle(x, z) {
if (!plannedPts || plannedPts.length < 2) return;
const remain = slicePathWithTrail(plannedPts, x, z, PLANNED_TRAIL_M);
plannedLine = drawTrajectoryLine(remain, 0x00ffaa, plannedLine);
routeLaneletBorder = drawRouteLaneletBorders(remain, routeLaneletBorder);
}
function alignGoalMeshToPlannedPath(planned) {
if (!poseState.goal) return;
if (ROBOT_STACK === 'nav2') return;
if (!goalMesh) {
goalMesh = createGoalMesh();
goalMesh.position.set(poseState.goal.pos.x, roadElevation + 0.16, poseState.goal.pos.z);
goalMesh.rotation.y = -(poseState.goal.heading * Math.PI / 180);
scene.add(goalMesh);
}
}
const PLAN_MAX_RETRY = 3;
const PLAN_RETRY_DELAY_MS = 3000;
let planGeneration = 0;
async function requestPlan(attempt = 1) {
if (!poseState.initial || !poseState.goal) return;
const gen = ++planGeneration;
if (attempt === 1) {
document.getElementById('btn-drive').disabled = true;
if (plannedLine) { scene.remove(plannedLine); plannedLine = null; }
if (routeLaneletBorder) { scene.remove(routeLaneletBorder); routeLaneletBorder = null; }
plannedPts = [];
routeUpdateFrame = 0;
}
if (ROBOT_STACK === 'nav2') {
setPlanStatus('Nav2 goal 전송 중...');
try {
const initRes = await fetch(`${ROS_BRIDGE_HTTP}/set_initial_pose`, {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify(threePoseToRosPayload(poseState.initial)),
});
if (gen !== planGeneration) return;
if (!initRes.ok) throw new Error(await initRes.text());
await new Promise(r => setTimeout(r, 300));
if (gen !== planGeneration) return;
const routeRes = await fetch(`${ROS_BRIDGE_HTTP}/set_route`, {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify(threePoseToRosPayload(poseState.goal)),
});
if (gen !== planGeneration) return;
if (!routeRes.ok) throw new Error(await routeRes.text());
const data = await routeRes.json().catch(() => ({}));
if (data.success === false) throw new Error(data.message || 'Nav2 goal rejected');
setPlanStatus('Nav2 goal 전송 완료');
document.getElementById('btn-drive').disabled = false;
connectNav2VisualizationBridge();
saveViewerState({ driveCompleted: false });
} catch (e) {
if (gen !== planGeneration) return;
setPlanStatus(`Nav2 goal 전송 실패: ${e.message}`);
}
return;
}
setPlanStatus(attempt === 1
? '계획 경로 요청 중...'
: `경로 초기화 후 재시도 중... (${attempt}/${PLAN_MAX_RETRY})`);
try {
const data = await requestAutowarePlanFromBridge();
if (gen !== planGeneration) return;
plannedPts = data.planned || [];
plannedLine = drawTrajectoryLine(plannedPts, 0x00ffaa, null);
routeLaneletBorder = drawRouteLaneletBorders(plannedPts, null);
alignGoalMeshToPlannedPath(plannedPts);
setPlanStatus(`계획 경로 수신 완료`);
document.getElementById('btn-drive').disabled = false;
saveViewerState({ driveCompleted: false });
if (pathHighlight) { scene.remove(pathHighlight); pathHighlight = null; }
} catch(e) {
if (gen !== planGeneration) return;
if (attempt < PLAN_MAX_RETRY) {
setPlanStatus(`오토웨어 리셋 후 재시도 중... (${attempt}/${PLAN_MAX_RETRY})`);
await fetch(`${ROS_BRIDGE_HTTP}/reset`, { method: 'POST' }).catch(() => {});
await new Promise(r => setTimeout(r, PLAN_RETRY_DELAY_MS));
if (gen !== planGeneration) return;
return requestPlan(attempt + 1);
}
setPlanStatus(`경로 계획 실패 (${PLAN_MAX_RETRY}회 시도): ${e.message}`);
}
}
function _driveFinished(msg) {
const btn = document.getElementById('btn-drive');
const completed = msg === '주행 완료';
setPlanStatus(msg);
hudSpeed = 0;
hudTurnState = 0;
hudSteeringAngle = 0;
hudPrevHeading = null;
const previousState = readViewerState();
saveViewerState({
driving: false,
driveCompleted: completed,
lastVehicle: previousState?.lastVehicle
? { ...previousState.lastVehicle, speed: 0, steer: 0, turn: 0 }
: null,
});
updateVehicleHud(0);
const icon = btn.querySelector('.pose-icon');
const label = btn.querySelector('.pose-label');
if (icon) icon.innerHTML = '&#9654;';
if (label) label.textContent = 'Start';
btn.disabled = completed;
btn.onclick = window.startDrive;
}
window.stopDrive = function() {
if (driveWs) { driveWs.close(); driveWs = null; }
fetch(`${ROS_BRIDGE_HTTP}/disengage`, { method: 'POST' }).catch(() => {});
saveViewerState({ driving: false, driveCompleted: false });
_driveFinished('주행 중지됨');
};
function applyKinematicSample(s, { drawActual = false } = {}) {
let { x, y, z } = s;
const { heading, speed, steer, turn } = s;
if (USE_ROS_MAP && Number.isFinite(s.map_x) && Number.isFinite(s.map_y) && Number.isFinite(s.map_z)) {
[x, y, z] = mapPayloadPointToThree([s.map_x, s.map_y, s.map_z], rosVectorPayload || null, 0);
}
if (!Number.isFinite(x) || !Number.isFinite(z) || !Number.isFinite(heading)) return null;
setVehicleVisualPoseFromRearAxle(x, z, heading, roadElevation + 0.8);
if (ROBOT_STACK !== 'nav2' && polarGrid) polarGrid.position.set(x, roadElevation + 0.03, z);
if (++routeUpdateFrame % 5 === 0) updateRemainingRouteByVehicle(x, z);
hudSpeed = hudSpeed * 0.75 + ((Number(speed) || 0) * 3.6) * 0.25;
const STEER_RATIO = 16;
hudSteeringAngle = hudSteeringAngle * 0.3 + (-(Number(steer) || 0) * STEER_RATIO) * 0.7;
hudSteeringAngle = Math.max(-540, Math.min(540, hudSteeringAngle));
hudTurnState = turn === 2 ? 1 : turn === 3 ? 2 : 0;
if (drawActual) {
const now = performance.now();
const ts = driveStartTime ? (now - driveStartTime) / 1000 : 0;
actualPts.push([x, y, z, ts]);
const trail = slicePathWithTrail(actualPts, x, z, ACTUAL_TRAIL_M);
actualLine = drawTrajectoryLine(trail, 0xff6600, actualLine, false);
}
saveViewerState({
lastVehicle: { x, y, z, heading, speed: Number(speed) || 0, steer: Number(steer) || 0, turn: Number(turn) || 0 },
driving: Boolean(driveWs),
driveCompleted: Boolean(readViewerState()?.driveCompleted && !driveWs),
});
return { x, y, z, heading };
}
function connectVehicleStateBridge() {
if (stateWs || driveWs) return;
try {
stateWs = new WebSocket(`${WS_BRIDGE}/ws/kinematic`);
stateWs.onmessage = (event) => {
if (driveWs) return;
applyKinematicSample(JSON.parse(event.data), { drawActual: false });
};
stateWs.onerror = () => { stateWs = null; };
stateWs.onclose = () => {
stateWs = null;
if (!driveWs) window.setTimeout(connectVehicleStateBridge, 1500);
};
} catch {}
}
window.startDrive = async function() {
if (!poseState.goal) return;
if (stateWs) { stateWs.close(); stateWs = null; }
const btn = document.getElementById('btn-drive');
btn.disabled = true;
const icon = btn.querySelector('.pose-icon');
const label = btn.querySelector('.pose-label');
if (icon) icon.innerHTML = '&#9632;';
if (label) label.textContent = 'Stop';
btn.onclick = window.stopDrive;
setPlanStatus('브리지 연결 중...');
if (actualLine) { scene.remove(actualLine); actualLine = null; }
actualPts = []; actualAnimIdx = 0;
routeUpdateFrame = 0;
try {
const res = await fetch(`${ROS_BRIDGE_HTTP}/engage`, {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify({ engage: true }),
});
if (!res.ok) throw new Error(`HTTP ${res.status}`);
setPlanStatus('자율주행 중...');
driveStartTime = performance.now();
driveWs = new WebSocket(`${WS_BRIDGE}/ws/kinematic`);
driveWs.onmessage = (event) => {
const s = JSON.parse(event.data);
const sample = applyKinematicSample(s, { drawActual: true });
if (!sample) return;
const { x, z } = sample;
const gdx = x - poseState.goal.pos.x;
const gdz = z - poseState.goal.pos.z;
if (Math.sqrt(gdx * gdx + gdz * gdz) < DRIVE_ARRIVAL_M) {
driveWs.close(); driveWs = null;
fetch(`${ROS_BRIDGE_HTTP}/disengage`, { method: 'POST' }).catch(() => {});
saveViewerState({ driving: false, driveCompleted: true });
_driveFinished('주행 완료');
connectVehicleStateBridge();
}
};
driveWs.onerror = () => {
driveWs = null;
fetch(`${ROS_BRIDGE_HTTP}/disengage`, { method: 'POST' }).catch(() => {});
saveViewerState({ driving: false, driveCompleted: false });
_driveFinished('WebSocket 오류 — 주행 중단');
connectVehicleStateBridge();
};
driveWs.onclose = () => { driveWs = null; connectVehicleStateBridge(); };
} catch(e) {
setPlanStatus(`주행 오류: ${e.message}`);
const icon = btn.querySelector('.pose-icon');
const label = btn.querySelector('.pose-label');
if (icon) icon.innerHTML = '&#9654;';
if (label) label.textContent = 'Start';
btn.disabled = false;
btn.onclick = window.startDrive;
connectVehicleStateBridge();
}
};
function restoreViewerState() {
const state = readViewerState();
if (!state || Date.now() - (state.updatedAt || 0) > 1000 * 60 * 60 * 6) return;
if (state.initial?.pos) {
applyPose('initial', new THREE.Vector3(state.initial.pos.x, state.initial.pos.y, state.initial.pos.z), state.initial.heading || 0, { skipPlan: true });
}
if (state.goal?.pos) {
applyPose('goal', new THREE.Vector3(state.goal.pos.x, state.goal.pos.y, state.goal.pos.z), state.goal.heading || 0, { skipPlan: true });
}
if (ROBOT_STACK !== 'nav2' && Array.isArray(state.planned) && state.planned.length >= 2) {
plannedPts = state.planned;
plannedLine = drawTrajectoryLine(plannedPts, 0x00ffaa, plannedLine);
routeLaneletBorder = drawRouteLaneletBorders(plannedPts, routeLaneletBorder);
alignGoalMeshToPlannedPath(plannedPts);
document.getElementById('btn-drive').disabled = Boolean(state.driveCompleted);
setPlanStatus(state.driveCompleted ? '주행 완료' : state.driving ? '자율주행 중...' : '계획 경로 복원됨');
}
if (state.lastVehicle) {
applyKinematicSample(state.lastVehicle, { drawActual: false });
if (ROBOT_STACK !== 'nav2' && !polarGrid) {
const center = rearAxleToVehicleCenterRad(state.lastVehicle.x, state.lastVehicle.z, state.lastVehicle.heading || 0);
createPolarGrid({ x: center.x, z: center.z });
}
}
}
// ── Mouse events ──
renderer.domElement.addEventListener('mousedown', e => {
if (!poseMode || e.button !== 0) return;
e.stopPropagation();
const pt = getGroundPoint(e);
if (!pt) return;
poseDragging = true; poseStartPoint = pt.clone();
});
renderer.domElement.addEventListener('mousemove', e => {
if (!poseDragging || !poseStartPoint) return;
const pt = getGroundPoint(e);
if (pt) updateTempArrow(poseStartPoint, pt);
});
renderer.domElement.addEventListener('mouseup', e => {
if (!poseDragging || !poseStartPoint || e.button !== 0) return;
poseDragging = false;
if (tempArrow) { scene.remove(tempArrow); tempArrow = null; }
const pt = getGroundPoint(e);
let heading = 0;
if (pt) {
const dx = pt.x - poseStartPoint.x, dz = pt.z - poseStartPoint.z;
if (Math.sqrt(dx*dx+dz*dz) > 0.5) heading = calcHeading(dx, dz);
}
const mode = poseMode;
applyPose(mode, poseStartPoint, heading);
poseStartPoint = null;
exitPoseMode();
});
// Escape cancels mode
window.addEventListener('keydown', e => {
if (e.key === 'Escape' && poseMode) {
if (tempArrow) { scene.remove(tempArrow); tempArrow = null; }
poseDragging = false; poseStartPoint = null;
exitPoseMode();
}
if (e.key === 'f' || e.key === 'F') {
const h = ROBOT_STACK === 'nav2' && rosVectorPayload ? getRosInitialCameraHeight(rosVectorPayload) : 80;
camera.position.set(START_X, h, START_Z + 0.01);
controls.target.set(START_X, 0, START_Z);
}
if (e.key === 't' || e.key === 'T') {
const t = controls.target.clone();
const h = ROBOT_STACK === 'nav2' && rosVectorPayload ? getRosInitialCameraHeight(rosVectorPayload) : 80;
camera.position.set(t.x, h, t.z + 0.01);
controls.target.set(t.x, 0, t.z);
}
});
function updateVehicleHud(dt) {
hudBlinkerT += dt;
const hud = document.getElementById('vehicle-hud');
if (!hud || !hud.classList.contains('visible')) return;
const canvas = document.getElementById('hud-steering');
if (canvas) {
const ctx = canvas.getContext('2d');
const cx = canvas.width/2, cy = canvas.height/2, r = 44;
ctx.clearRect(0, 0, canvas.width, canvas.height);
ctx.save(); ctx.translate(cx, cy); ctx.rotate(hudSteeringAngle * Math.PI / 180);
ctx.beginPath(); ctx.arc(0, 0, r, 0, Math.PI*2);
ctx.strokeStyle = '#7fbbff'; ctx.lineWidth = 4; ctx.stroke();
ctx.beginPath(); ctx.arc(0, 0, r*0.18, 0, Math.PI*2);
ctx.strokeStyle = 'rgba(127,187,255,0.5)'; ctx.lineWidth = 2; ctx.fill(); ctx.stroke();
// T자: 좌(180°), 우(0°), 아래(270°)
for (const deg of [0, 180, 90]) {
const a = deg * Math.PI / 180;
ctx.beginPath();
ctx.moveTo(Math.cos(a)*r*0.18, Math.sin(a)*r*0.18);
ctx.lineTo(Math.cos(a)*r, Math.sin(a)*r);
ctx.strokeStyle = '#7fbbff'; ctx.lineWidth = 3.5; ctx.stroke();
}
ctx.restore();
ctx.save(); ctx.translate(cx, cy);
ctx.beginPath(); ctx.moveTo(0,-r-3); ctx.lineTo(-4,-r-10); ctx.lineTo(4,-r-10); ctx.closePath();
ctx.fillStyle = '#88ffcc'; ctx.fill(); ctx.restore();
}
const steerVal = document.getElementById('hud-steer-val');
if (steerVal) steerVal.textContent = (hudSteeringAngle / -15).toFixed(1);
const gc = document.getElementById('hud-speed-gauge-canvas');
if (gc) {
const gx = gc.getContext('2d');
const gw = gc.width, gh = gc.height;
gx.clearRect(0, 0, gw, gh);
const ccx = gw/2, ccy = gh-8;
const gr = Math.min(gw/2-5, gh-14);
const startA = Math.PI, endA = Math.PI*2;
const ratio = Math.min(Math.abs(hudSpeed)/80, 1);
const needleA = startA + (endA-startA)*ratio;
gx.beginPath(); gx.arc(ccx, ccy, gr, startA, endA);
gx.strokeStyle = '#223344'; gx.lineWidth = 6; gx.stroke();
gx.beginPath(); gx.arc(ccx, ccy, gr, startA, needleA);
gx.strokeStyle = '#88ffcc'; gx.lineWidth = 6; gx.stroke();
for (let i = 0; i <= 8; i++) {
const a = startA + (endA-startA)*(i/8);
const isMain = i%2===0;
const r1=gr+3, r2=gr-(isMain?8:4);
gx.beginPath(); gx.moveTo(ccx+Math.cos(a)*r1, ccy+Math.sin(a)*r1);
gx.lineTo(ccx+Math.cos(a)*r2, ccy+Math.sin(a)*r2);
gx.strokeStyle = isMain?'#556677':'#334455'; gx.lineWidth=isMain?1.5:1; gx.stroke();
}
gx.beginPath(); gx.moveTo(ccx, ccy);
gx.lineTo(ccx+Math.cos(needleA)*(gr-4), ccy+Math.sin(needleA)*(gr-4));
gx.strokeStyle='#ffcc44'; gx.lineWidth=2; gx.lineCap='round'; gx.stroke();
gx.beginPath(); gx.arc(ccx, ccy, 3, 0, Math.PI*2);
gx.fillStyle='#ffcc44'; gx.fill();
}
const speedEl = document.getElementById('hud-speed-val');
if (speedEl) speedEl.textContent = hudSpeed.toFixed(2);
const limitEl = document.getElementById('hud-limit-val');
if (limitEl) limitEl.textContent = hudSpeedLimit !== null ? hudSpeedLimit+'km/h' : '-';
const blink = (hudBlinkerT % 1.0) < 0.5;
const leftEl = document.getElementById('hud-left-sig');
const rightEl = document.getElementById('hud-right-sig');
if (leftEl) leftEl.className = 'hud-signal'+(hudTurnState===1&&blink?' active':'');
if (rightEl) rightEl.className = 'hud-signal'+(hudTurnState===2&&blink?' active':'');
}
// ──────────────────────────────────────────────────────
// Resize & Render loop
// ──────────────────────────────────────────────────────
window.addEventListener('resize', () => {
camera.aspect = window.innerWidth / window.innerHeight;
camera.updateProjectionMatrix();
renderer.setSize(window.innerWidth, window.innerHeight);
});
(function animate() {
requestAnimationFrame(animate);
const delta = animClock.getDelta();
controls.update();
updatePolarPulse();
updateTrafficLights(delta);
updateVehicleHud(delta);
// 거리 기반 컬링 (나무/건물, 10프레임마다)
if (Math.floor(performance.now() / 160) % 10 === 0) {
const TREE_DIST = 120, BLDG_DIST = 180;
const cp = camera.position;
if (layerObjects.tree && layerState.tree) {
for (const child of layerObjects.tree.children) {
const dx = child.position.x - cp.x, dz = child.position.z - cp.z;
child.visible = (dx*dx + dz*dz) < TREE_DIST*TREE_DIST;
}
}
if (layerObjects.building && layerState.building) {
const tmp = new THREE.Vector3();
for (const child of layerObjects.building.children) {
child.getWorldPosition(tmp);
const dx = tmp.x - cp.x, dz = tmp.z - cp.z;
child.visible = (dx*dx + dz*dz) < BLDG_DIST*BLDG_DIST;
}
}
}
// 메인 뷰
renderer.setScissorTest(false);
renderer.setViewport(0, 0, window.innerWidth, window.innerHeight);
renderer.render(scene, camera);
})();
// ──────────────────────────────────────────────────────
// Main
// ──────────────────────────────────────────────────────
(async () => {
try {
if (USE_ROS_MAP) {
setText('s-points', 'waiting');
await connectRosMapBridge();
} else {
if (USE_PCD_ON_START) {
layerState.pointcloud = true;
const chk = document.getElementById('chk-pointcloud');
if (chk) chk.className = 'layer-check on';
await loadPointCloud();
} else {
setText('s-points', 'off');
setProgress(10, 'PointCloud skipped (optional).');
}
await loadLanelet2();
}
await loadVehicleGlb();
document.getElementById('loading').style.display = 'none';
document.getElementById('panel-wrap').style.display = 'block';
document.getElementById('colorbar-wrap').style.display = USE_PCD_ON_START ? 'block' : 'none';
document.getElementById('pose-toolbar').style.display = 'flex';
document.getElementById('vehicle-hud').classList.add('visible');
restoreViewerState();
connectNav2VisualizationBridge();
connectVehicleStateBridge();
} catch (err) {
console.error(err);
}
})();
</script>
</body>
</html>