3421 lines
123 KiB
HTML
3421 lines
123 KiB
HTML
<!DOCTYPE html>
|
|
<html lang="ko">
|
|
<head>
|
|
<meta charset="UTF-8" />
|
|
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
|
|
<title>TurtleBot3 World Viewer</title>
|
|
<style>
|
|
html, body { margin:0; width:100%; height:100%; overflow:hidden; background:#9f9f9f; }
|
|
canvas { display:block; }
|
|
#miniMap {
|
|
position:absolute;
|
|
right:16px;
|
|
bottom:16px;
|
|
width:260px;
|
|
height:220px;
|
|
z-index:10;
|
|
background:#111520;
|
|
border:1px solid rgba(0,0,0,.45);
|
|
pointer-events:none;
|
|
}
|
|
#miniMap.hidden {
|
|
display:none;
|
|
}
|
|
#miniMapToggle {
|
|
position:absolute;
|
|
right:16px;
|
|
bottom:240px;
|
|
z-index:11;
|
|
min-height:28px;
|
|
padding:0 10px;
|
|
border:1px solid rgba(255,255,255,.22);
|
|
background:rgba(17,24,39,.42);
|
|
color:rgba(255,255,255,.88);
|
|
font:12px sans-serif;
|
|
font-weight:800;
|
|
cursor:pointer;
|
|
backdrop-filter:blur(10px);
|
|
box-shadow:0 10px 26px rgba(0,0,0,.16);
|
|
transition:background .15s ease, border-color .15s ease, color .15s ease;
|
|
}
|
|
#miniMapToggle:hover {
|
|
background:rgba(17,24,39,.58);
|
|
border-color:rgba(255,255,255,.34);
|
|
color:#fff;
|
|
}
|
|
#miniMap.hidden + #miniMapToggle {
|
|
bottom:16px;
|
|
}
|
|
#cameraPreviewWrap {
|
|
position:absolute;
|
|
right:16px;
|
|
bottom:256px;
|
|
z-index:10;
|
|
display:none;
|
|
width:260px;
|
|
border:1px solid rgba(0,0,0,.45);
|
|
background:#111520;
|
|
box-shadow:0 10px 26px rgba(0,0,0,.16);
|
|
}
|
|
#cameraPreviewWrap.is-visible { display:block; }
|
|
#cameraPreviewWrap.is-collapsed { background:transparent; border-color:transparent; box-shadow:none; }
|
|
#cameraPreview {
|
|
display:block;
|
|
width:260px;
|
|
aspect-ratio:16 / 9;
|
|
object-fit:cover;
|
|
background:#111520;
|
|
}
|
|
#cameraPreviewWrap.is-collapsed #cameraPreview { display:none; }
|
|
#cameraPreviewToggle {
|
|
position:absolute;
|
|
right:0;
|
|
top:-28px;
|
|
display:grid;
|
|
width:30px;
|
|
height:28px;
|
|
place-items:center;
|
|
padding:0;
|
|
border:1px solid rgba(255,255,255,.22);
|
|
background:rgba(17,24,39,.42);
|
|
color:rgba(255,255,255,.9);
|
|
font:11px sans-serif;
|
|
font-weight:800;
|
|
cursor:pointer;
|
|
backdrop-filter:blur(8px);
|
|
box-shadow:0 10px 28px rgba(0,0,0,.16);
|
|
}
|
|
#cameraPreviewToggle:hover { color:#fff; background:rgba(255,255,255,.06); }
|
|
#cameraPreviewChevron { font-size:13px; line-height:1; }
|
|
#cameraPreviewWrap.is-collapsed #cameraPreviewChevron { transform:rotate(180deg); }
|
|
#topRightControls {
|
|
position:absolute;
|
|
right:16px;
|
|
top:16px;
|
|
z-index:10;
|
|
display:flex;
|
|
gap:7px;
|
|
align-items:flex-start;
|
|
}
|
|
#topLeftControls {
|
|
position:absolute;
|
|
left:16px;
|
|
top:16px;
|
|
z-index:10;
|
|
display:none;
|
|
}
|
|
#keyGuide {
|
|
position:absolute;
|
|
left:16px;
|
|
bottom:16px;
|
|
z-index:10;
|
|
padding:7px 9px;
|
|
border:1px solid rgba(255,255,255,.24);
|
|
background:rgba(17,24,39,.34);
|
|
color:rgba(255,255,255,.82);
|
|
font:11px/1.35 sans-serif;
|
|
pointer-events:none;
|
|
backdrop-filter:blur(8px);
|
|
box-shadow:0 10px 28px rgba(0,0,0,.16);
|
|
}
|
|
#rosTopicStatusWrap {
|
|
position:absolute;
|
|
left:16px;
|
|
bottom:54px;
|
|
z-index:10;
|
|
width:178px;
|
|
}
|
|
#rosTopicStatus {
|
|
overflow:hidden;
|
|
border:1px solid rgba(255,255,255,.24);
|
|
background:rgba(17,24,39,.48);
|
|
color:rgba(255,255,255,.88);
|
|
font:11px sans-serif;
|
|
backdrop-filter:blur(8px);
|
|
box-shadow:0 10px 28px rgba(0,0,0,.16);
|
|
}
|
|
#rosTopicStatusToggle {
|
|
position:absolute;
|
|
left:0;
|
|
top:-28px;
|
|
display:grid;
|
|
width:30px;
|
|
height:28px;
|
|
place-items:center;
|
|
padding:0;
|
|
border:1px solid rgba(255,255,255,.22);
|
|
background:rgba(17,24,39,.42);
|
|
color:rgba(255,255,255,.9);
|
|
font:11px sans-serif;
|
|
font-weight:800;
|
|
cursor:pointer;
|
|
backdrop-filter:blur(8px);
|
|
box-shadow:0 10px 28px rgba(0,0,0,.16);
|
|
}
|
|
#rosTopicStatusToggle:hover { color:#fff; background:rgba(255,255,255,.06); }
|
|
#rosTopicStatusWrap.is-collapsed #rosTopicStatus { display:none; }
|
|
#rosTopicStatusChevron { font-size:13px; line-height:1; }
|
|
#rosTopicStatusWrap.is-collapsed #rosTopicStatusChevron { transform:rotate(180deg); }
|
|
#rosTopicStatusTitle {
|
|
min-height:30px;
|
|
display:flex;
|
|
align-items:center;
|
|
padding:0 9px;
|
|
border-bottom:1px solid rgba(255,255,255,.14);
|
|
font-weight:800;
|
|
}
|
|
#rosTopicStatusList { padding:4px 0; }
|
|
.ros-topic-row {
|
|
display:flex;
|
|
align-items:center;
|
|
justify-content:space-between;
|
|
gap:8px;
|
|
min-height:23px;
|
|
padding:0 9px;
|
|
}
|
|
.ros-topic-name { overflow:hidden; text-overflow:ellipsis; white-space:nowrap; }
|
|
.ros-topic-state { flex:0 0 auto; font-size:12px; font-weight:900; }
|
|
.ros-topic-state.is-ok { color:#5eead4; }
|
|
.ros-topic-state.is-error { color:#fca5a5; }
|
|
#responseWaitOverlay {
|
|
position:fixed;
|
|
inset:0;
|
|
z-index:30;
|
|
display:flex;
|
|
align-items:center;
|
|
justify-content:center;
|
|
background:rgba(5,8,16,.58);
|
|
backdrop-filter:blur(2px);
|
|
transition:opacity .2s ease;
|
|
}
|
|
#responseWaitOverlay.is-hidden {
|
|
opacity:0;
|
|
pointer-events:none;
|
|
}
|
|
#mapSaveOverlay {
|
|
position:fixed;
|
|
inset:0;
|
|
z-index:40;
|
|
display:flex;
|
|
align-items:center;
|
|
justify-content:center;
|
|
background:rgba(5,8,16,.46);
|
|
backdrop-filter:blur(2px);
|
|
opacity:0;
|
|
pointer-events:none;
|
|
transition:opacity .16s ease;
|
|
}
|
|
#mapSaveOverlay.is-visible {
|
|
opacity:1;
|
|
pointer-events:auto;
|
|
}
|
|
.response-wait-dialog {
|
|
display:flex;
|
|
align-items:center;
|
|
gap:10px;
|
|
padding:14px 18px;
|
|
border:1px solid rgba(255,255,255,.16);
|
|
border-radius:8px;
|
|
background:rgba(17,21,32,.94);
|
|
color:rgba(255,255,255,.9);
|
|
font:13px sans-serif;
|
|
font-weight:700;
|
|
box-shadow:0 18px 48px rgba(0,0,0,.3);
|
|
}
|
|
.response-wait-spinner {
|
|
width:16px;
|
|
height:16px;
|
|
border:2px solid rgba(255,255,255,.22);
|
|
border-top-color:#3b7ffa;
|
|
border-radius:50%;
|
|
animation:response-wait-spin .75s linear infinite;
|
|
}
|
|
@keyframes response-wait-spin {
|
|
to { transform:rotate(360deg); }
|
|
}
|
|
#downloadWrap {
|
|
position:relative;
|
|
}
|
|
#downloadMap,
|
|
#pathToggle,
|
|
#sensorToggle,
|
|
#poseToggle,
|
|
#goalToggle,
|
|
#obstacleToggle,
|
|
#cameraPresetSelect {
|
|
min-height:32px;
|
|
border:1px solid rgba(255,255,255,.22);
|
|
background:rgba(17,24,39,.42);
|
|
color:rgba(255,255,255,.88);
|
|
font:12px sans-serif;
|
|
font-weight:700;
|
|
cursor:pointer;
|
|
backdrop-filter:blur(10px);
|
|
box-shadow:0 10px 26px rgba(0,0,0,.16);
|
|
transition:background .15s ease, border-color .15s ease, color .15s ease;
|
|
}
|
|
#downloadMap,
|
|
#pathToggle,
|
|
#sensorToggle,
|
|
#poseToggle,
|
|
#goalToggle,
|
|
#obstacleToggle {
|
|
padding:0 11px;
|
|
}
|
|
#downloadMap {
|
|
display:inline-flex;
|
|
align-items:center;
|
|
gap:6px;
|
|
}
|
|
#cameraPresetSelect {
|
|
min-width:86px;
|
|
padding:0 24px 0 9px;
|
|
outline:none;
|
|
appearance:none;
|
|
background-image:linear-gradient(45deg, transparent 50%, rgba(255,255,255,.7) 50%), linear-gradient(135deg, rgba(255,255,255,.7) 50%, transparent 50%);
|
|
background-position:calc(100% - 13px) 14px, calc(100% - 8px) 14px;
|
|
background-size:5px 5px, 5px 5px;
|
|
background-repeat:no-repeat;
|
|
}
|
|
#downloadMap:hover,
|
|
#pathToggle:hover,
|
|
#sensorToggle:hover,
|
|
#poseToggle:hover,
|
|
#goalToggle:hover,
|
|
#obstacleToggle:hover,
|
|
#cameraPresetSelect:hover {
|
|
background-color:rgba(17,24,39,.58);
|
|
border-color:rgba(255,255,255,.34);
|
|
color:#fff;
|
|
}
|
|
#downloadMap:disabled {
|
|
opacity:.45;
|
|
cursor:not-allowed;
|
|
}
|
|
#goalToggle:disabled {
|
|
opacity:.42;
|
|
cursor:not-allowed;
|
|
}
|
|
#poseToggle:disabled {
|
|
opacity:.32;
|
|
cursor:not-allowed;
|
|
}
|
|
#pathToggle.active {
|
|
background:rgba(37,99,235,.62);
|
|
border-color:rgba(147,197,253,.55);
|
|
color:#fff;
|
|
}
|
|
#sensorToggle.active {
|
|
background:rgba(14,116,144,.62);
|
|
border-color:rgba(103,232,249,.55);
|
|
color:#fff;
|
|
}
|
|
#goalToggle.active {
|
|
background:rgba(22,163,74,.62);
|
|
border-color:rgba(134,239,172,.55);
|
|
color:#fff;
|
|
}
|
|
#poseToggle.active {
|
|
background:rgba(37,99,235,.62);
|
|
border-color:rgba(147,197,253,.55);
|
|
color:#fff;
|
|
}
|
|
#obstacleToggle.active {
|
|
background:rgba(217,119,6,.62);
|
|
border-color:rgba(253,230,138,.62);
|
|
color:#fff;
|
|
}
|
|
#downloadMenu {
|
|
position:absolute;
|
|
right:0;
|
|
top:38px;
|
|
min-width:104px;
|
|
display:none;
|
|
overflow:hidden;
|
|
border:1px solid rgba(255,255,255,.22);
|
|
background:rgba(17,24,39,.72);
|
|
backdrop-filter:blur(10px);
|
|
box-shadow:0 14px 32px rgba(0,0,0,.22);
|
|
}
|
|
#downloadMenu.open { display:block; }
|
|
#downloadMenu button {
|
|
display:block;
|
|
width:100%;
|
|
padding:8px 11px;
|
|
border:0;
|
|
background:transparent;
|
|
color:rgba(255,255,255,.82);
|
|
text-align:left;
|
|
font:12px sans-serif;
|
|
font-weight:700;
|
|
cursor:pointer;
|
|
}
|
|
#downloadMenu button:hover {
|
|
background:rgba(255,255,255,.1);
|
|
color:#fff;
|
|
}
|
|
.editor-mode #topRightControls,
|
|
.editor-mode #miniMap,
|
|
.editor-mode #miniMapToggle,
|
|
.editor-mode #cameraPreviewWrap,
|
|
.editor-mode #keyGuide,
|
|
.editor-mode #rosTopicStatusWrap {
|
|
display:none;
|
|
}
|
|
.nav2-slam #goalToggle, .nav2-slam #poseToggle { display:none; }
|
|
.nav2-navigation #downloadWrap { display:none; }
|
|
.nav2-navigation #topLeftControls { display:block; }
|
|
.nav2-navigation #keyGuide { display:none; }
|
|
.nav2-navigation #rosTopicStatusWrap { bottom:16px; }
|
|
.real-runtime #topLeftControls { display:none; }
|
|
.real-runtime #keyGuide { display:none; }
|
|
</style>
|
|
</head>
|
|
<body>
|
|
<div id="topLeftControls">
|
|
<button id="obstacleToggle" title="벽 배치" aria-label="벽 배치" aria-pressed="false">Wall</button>
|
|
</div>
|
|
<div id="topRightControls">
|
|
<div id="downloadWrap">
|
|
<button id="downloadMap" title="맵 저장" aria-label="맵 저장" disabled><span>SAVE</span></button>
|
|
</div>
|
|
<button id="pathToggle" title="로봇 경로 표시" aria-label="로봇 경로 표시">Path</button>
|
|
<button id="sensorToggle" title="센서 표시" aria-label="센서 표시" aria-pressed="false">Sensor</button>
|
|
<button id="poseToggle" title="초기 위치 지정" aria-label="초기 위치 지정">Pose</button>
|
|
<button id="goalToggle" title="목표 위치 지정" aria-label="목표 위치 지정">Goal</button>
|
|
<select id="cameraPresetSelect" title="카메라 시점" aria-label="카메라 시점">
|
|
<option value="default">Default</option>
|
|
<option value="top">Top</option>
|
|
<option value="follow">Follow</option>
|
|
</select>
|
|
</div>
|
|
<canvas id="miniMap" width="260" height="220"></canvas>
|
|
<button id="miniMapToggle" title="미니맵 접기/펼치기" aria-label="미니맵 접기">▾</button>
|
|
<div id="cameraPreviewWrap">
|
|
<button id="cameraPreviewToggle" type="button" title="카메라 화면 접기/펼치기" aria-label="카메라 화면 접기" aria-expanded="true"><span id="cameraPreviewChevron" aria-hidden="true">▾</span></button>
|
|
<img id="cameraPreview" alt="실시간 카메라 화면" />
|
|
</div>
|
|
<div id="keyGuide">
|
|
<div>i 전진 · , 후진 · j/l 회전 · k 정지</div>
|
|
</div>
|
|
<div id="rosTopicStatusWrap">
|
|
<button id="rosTopicStatusToggle" type="button" title="ROS 상태 접기/펼치기" aria-label="ROS 상태 접기" aria-expanded="true">
|
|
<span id="rosTopicStatusChevron" aria-hidden="true">▾</span>
|
|
</button>
|
|
<aside id="rosTopicStatus" aria-label="ROS 상태">
|
|
<div id="rosTopicStatusTitle">ROS 상태</div>
|
|
<div id="rosTopicStatusList"></div>
|
|
</aside>
|
|
</div>
|
|
<div id="responseWaitOverlay" role="status" aria-live="polite">
|
|
<div class="response-wait-dialog">
|
|
<span class="response-wait-spinner" aria-hidden="true"></span>
|
|
<span>응답을 기다리는 중입니다.</span>
|
|
</div>
|
|
</div>
|
|
<div id="mapSaveOverlay" role="status" aria-live="polite" aria-hidden="true">
|
|
<div class="response-wait-dialog">
|
|
<span class="response-wait-spinner" aria-hidden="true"></span>
|
|
<span>맵을 저장하고 있습니다.</span>
|
|
</div>
|
|
</div>
|
|
<script type="importmap">
|
|
{
|
|
"imports": {
|
|
"three": "../vendor/three/build/three.module.js",
|
|
"three/addons/": "../vendor/three/examples/jsm/"
|
|
}
|
|
}
|
|
</script>
|
|
<script type="module">
|
|
import * as THREE from 'three';
|
|
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
|
|
import { TransformControls } from 'three/addons/controls/TransformControls.js';
|
|
import { ColladaLoader } from 'three/addons/loaders/ColladaLoader.js';
|
|
import { GLTFLoader } from 'three/addons/loaders/GLTFLoader.js';
|
|
import { STLLoader } from 'three/addons/loaders/STLLoader.js';
|
|
|
|
const viewerMapKey = new URLSearchParams(window.location.search).get('mapKey');
|
|
const params = new URLSearchParams(location.search);
|
|
const REAL_RUNTIME = params.get('runtime') === 'real';
|
|
const ROBOT_PLATFORM = (params.get('platform') || '').trim().toLowerCase();
|
|
// Keep the prior real-platform default when an older LMS URL has no platform.
|
|
const USE_MKMINI_MODEL = ROBOT_PLATFORM === 'mkmini' || (!ROBOT_PLATFORM && REAL_RUNTIME);
|
|
const SHOW_POINT_CLOUD = params.get('pointCloud') === '1';
|
|
const SHOW_SENSOR_POINT_CLOUD = REAL_RUNTIME && SHOW_POINT_CLOUD;
|
|
const SENSOR_POINT_CLOUD_MAX_AGE_MS = 3_000;
|
|
// Camera preview is kept for the RGB test module, but is paused for now so
|
|
// regular visualization sessions never poll `/camera-frame`.
|
|
const ENABLE_CAMERA_PREVIEW = false;
|
|
const CUSTOM_WORLD = !REAL_RUNTIME && viewerMapKey === 'nav2-custom';
|
|
const SAVED_GAZEBO_MAP_WORLD = !REAL_RUNTIME && viewerMapKey === 'nav2-saved-gazebo-map';
|
|
const activeWorldModel = viewerMapKey === 'nav2-turtlebot3-house' ? 'turtlebot3_house' : 'turtlebot3_world';
|
|
const MKMINI_WORLD_SINGLE_PILLAR = USE_MKMINI_MODEL && activeWorldModel === 'turtlebot3_world';
|
|
const MKMINI_RENDER_LENGTH = 0.84;
|
|
const MKMINI_RENDER_WIDTH = 0.61;
|
|
const MKMINI_RENDER_HEIGHT = 0.6575;
|
|
const MKMINI_WHEELBASE_METERS = 0.6;
|
|
const MKMINI_MAX_STEERING_RADIANS = 0.52;
|
|
const DEFAULT_SIMULATION_SPAWN = USE_MKMINI_MODEL
|
|
? { x: -1.4, y: -0.5 }
|
|
: { x: -2.0, y: -0.5 };
|
|
const MODEL_ROOT = `./turtlebot3_world_model/${activeWorldModel}`;
|
|
const MODEL_SDF = `${MODEL_ROOT}/model.sdf`;
|
|
const miniMap = document.getElementById('miniMap');
|
|
const miniCtx = miniMap.getContext('2d');
|
|
const miniMapToggle = document.getElementById('miniMapToggle');
|
|
const cameraPreviewWrap = document.getElementById('cameraPreviewWrap');
|
|
const cameraPreview = document.getElementById('cameraPreview');
|
|
const cameraPreviewToggle = document.getElementById('cameraPreviewToggle');
|
|
const responseWaitOverlay = document.getElementById('responseWaitOverlay');
|
|
const mapSaveOverlay = document.getElementById('mapSaveOverlay');
|
|
const downloadMapButton = document.getElementById('downloadMap');
|
|
const pathToggleButton = document.getElementById('pathToggle');
|
|
const sensorToggleButton = document.getElementById('sensorToggle');
|
|
const poseToggleButton = document.getElementById('poseToggle');
|
|
const goalToggleButton = document.getElementById('goalToggle');
|
|
const obstacleToggleButton = document.getElementById('obstacleToggle');
|
|
const cameraPresetSelect = document.getElementById('cameraPresetSelect');
|
|
const rosTopicStatusWrap = document.getElementById('rosTopicStatusWrap');
|
|
const rosTopicStatus = document.getElementById('rosTopicStatus');
|
|
const rosTopicStatusToggle = document.getElementById('rosTopicStatusToggle');
|
|
const rosTopicStatusList = document.getElementById('rosTopicStatusList');
|
|
const EDITOR_MODE = params.get('editor') === '1';
|
|
const NAV2_VIEW_MODE = params.get('mode') === 'navigation' ? 'navigation' : 'slam';
|
|
// A navigation session must keep the selected saved map as its fixed
|
|
// reference. The runtime /map topic may later emit a partial local update.
|
|
const LOCK_NAVIGATION_MAP = REAL_RUNTIME && NAV2_VIEW_MODE === 'navigation';
|
|
const DIRECT_BRIDGE = params.get('directBridge') === '1';
|
|
const BACKEND_URL = params.get('backend') || params.get('bridgeHttp') || 'http://localhost:8765';
|
|
const BACKEND_WS = params.get('bridgeWs')
|
|
|| BACKEND_URL.replace(/^http:/, 'ws:').replace(/^https:/, 'wss:') + (DIRECT_BRIDGE ? '/ws/state' : '/ws/slam');
|
|
const CONTROL_WS = DIRECT_BRIDGE ? BACKEND_WS.replace(/\/ws\/state(?:\?.*)?$/, '/ws/control') : null;
|
|
if (EDITOR_MODE) document.body.classList.add('editor-mode');
|
|
if (!EDITOR_MODE) document.body.classList.add(`nav2-${NAV2_VIEW_MODE}`);
|
|
if (REAL_RUNTIME) document.body.classList.add('real-runtime');
|
|
if (EDITOR_MODE) responseWaitOverlay?.classList.add('is-hidden');
|
|
|
|
let visualizationResponseReceived = EDITOR_MODE;
|
|
const ROS_TOPIC_STATUS_KEYS = NAV2_VIEW_MODE === 'navigation'
|
|
? [
|
|
...(SHOW_POINT_CLOUD ? ['sensor_point_cloud', 'point_cloud'] : ['scan']),
|
|
'odom', 'map', 'plan', 'cmd_vel', 'navigate_to_pose',
|
|
]
|
|
: SHOW_POINT_CLOUD
|
|
? ['sensor_point_cloud', 'point_cloud', 'odom', 'map']
|
|
: ['scan', 'odom', 'map'];
|
|
const ROS_TOPIC_STATUS_FALLBACK_NAMES = {
|
|
scan: '/scan',
|
|
odom: '/odom',
|
|
map: '/map',
|
|
amcl_pose: '/amcl_pose',
|
|
plan: '/plan',
|
|
cmd_vel: '/cmd_vel',
|
|
navigate_to_pose: '/navigate_to_pose',
|
|
point_cloud: '/cloud_map',
|
|
sensor_point_cloud: '/lidar_filtered',
|
|
};
|
|
|
|
function updateRosTopicStatus(topicStatus = {}) {
|
|
if (!rosTopicStatusList) return;
|
|
rosTopicStatusList.replaceChildren(...ROS_TOPIC_STATUS_KEYS.map((key) => {
|
|
const status = topicStatus?.[key] ?? {};
|
|
const row = document.createElement('div');
|
|
row.className = 'ros-topic-row';
|
|
const name = document.createElement('span');
|
|
name.className = 'ros-topic-name';
|
|
name.textContent = status.name || ROS_TOPIC_STATUS_FALLBACK_NAMES[key] || key;
|
|
const indicator = document.createElement('span');
|
|
const available = status.available === true;
|
|
indicator.className = `ros-topic-state ${available ? 'is-ok' : 'is-error'}`;
|
|
indicator.textContent = available ? 'O' : 'X';
|
|
row.append(name, indicator);
|
|
return row;
|
|
}));
|
|
|
|
// Do not leave a previous lidar frame on screen after the bridge has marked
|
|
// its source stale. A stale point cloud looks like a map/laser transform bug.
|
|
if (Object.keys(topicStatus).length && topicStatus?.sensor_point_cloud?.available !== true) {
|
|
clearSensorPointCloud();
|
|
}
|
|
}
|
|
|
|
rosTopicStatusToggle?.addEventListener('click', () => {
|
|
const collapsed = rosTopicStatusWrap?.classList.toggle('is-collapsed') ?? false;
|
|
rosTopicStatusToggle.setAttribute('aria-expanded', String(!collapsed));
|
|
rosTopicStatusToggle.setAttribute('aria-label', collapsed ? 'ROS 상태 펼치기' : 'ROS 상태 접기');
|
|
});
|
|
updateRosTopicStatus();
|
|
|
|
function markVisualizationResponseReceived() {
|
|
if (visualizationResponseReceived) return;
|
|
visualizationResponseReceived = true;
|
|
responseWaitOverlay?.classList.add('is-hidden');
|
|
}
|
|
|
|
function setMapSavePending(isPending) {
|
|
mapSaveOverlay?.classList.toggle('is-visible', isPending);
|
|
mapSaveOverlay?.setAttribute('aria-hidden', String(!isPending));
|
|
}
|
|
|
|
function focusViewer() {
|
|
document.activeElement?.blur?.();
|
|
renderer.domElement.focus();
|
|
}
|
|
|
|
const scene = new THREE.Scene();
|
|
scene.background = new THREE.Color(0x9f9f9f);
|
|
const camera = new THREE.PerspectiveCamera(45, innerWidth / innerHeight, 0.05, 1000);
|
|
// Three.js OrbitControls 기본 조작이 자연스럽게 동작하도록 사선 시점에서 시작합니다.
|
|
camera.position.set(4, 6, 5);
|
|
camera.up.set(0, 1, 0);
|
|
const renderer = new THREE.WebGLRenderer({ antialias:true });
|
|
renderer.setSize(innerWidth, innerHeight);
|
|
renderer.setPixelRatio(Math.min(devicePixelRatio, 1.25));
|
|
renderer.shadowMap.enabled = true;
|
|
renderer.domElement.tabIndex = 0;
|
|
document.body.appendChild(renderer.domElement);
|
|
|
|
const controls = new OrbitControls(camera, renderer.domElement);
|
|
controls.mouseButtons = {
|
|
LEFT: THREE.MOUSE.PAN,
|
|
MIDDLE: THREE.MOUSE.ROTATE,
|
|
RIGHT: THREE.MOUSE.DOLLY,
|
|
};
|
|
renderer.domElement.addEventListener('contextmenu', (event) => {
|
|
event.preventDefault();
|
|
if (!obstaclePlacementMode) return;
|
|
obstaclePlacementMode = false;
|
|
obstacleToggleButton.classList.remove('active');
|
|
obstacleToggleButton.setAttribute('aria-pressed', 'false');
|
|
hideObstaclePreview();
|
|
focusViewer();
|
|
});
|
|
controls.enableDamping = true;
|
|
controls.dampingFactor = 0.08;
|
|
controls.target.set(0, 0, 0);
|
|
camera.lookAt(controls.target);
|
|
|
|
scene.add(new THREE.AmbientLight(0xffffff, .65));
|
|
const sun = new THREE.DirectionalLight(0xffffff, .85);
|
|
sun.position.set(3, 6, 4);
|
|
sun.castShadow = true;
|
|
sun.shadow.mapSize.set(512, 512);
|
|
scene.add(sun);
|
|
|
|
// Three.js 화면용 바닥/격자입니다. 실제 SDF 모델과 별개입니다.
|
|
const floor = new THREE.Mesh(
|
|
new THREE.PlaneGeometry(20, 20),
|
|
new THREE.MeshLambertMaterial({ color: 0xd0d0d0 })
|
|
);
|
|
floor.rotation.x = -Math.PI / 2;
|
|
floor.position.y = -0.01;
|
|
floor.receiveShadow = true;
|
|
scene.add(floor);
|
|
scene.add(new THREE.GridHelper(20, 20, 0x333333, 0x555555));
|
|
|
|
// SDF/Gazebo는 Z-up, Three.js는 Y-up입니다.
|
|
// 그래서 모든 SDF 객체를 이 그룹 안에 원래 SDF 좌표 그대로 넣고,
|
|
// 그룹 전체만 -90도 회전시켜 Three.js 좌표계로 변환합니다.
|
|
const sdfRoot = new THREE.Group();
|
|
sdfRoot.rotation.x = -Math.PI / 2;
|
|
scene.add(sdfRoot);
|
|
// Gazebo world geometry is authored in `odom`. Keep it separate from the
|
|
// map-frame robot, scan, route and occupancy-map layers.
|
|
const gazeboWorldRoot = new THREE.Group();
|
|
gazeboWorldRoot.name = 'gazebo-world';
|
|
sdfRoot.add(gazeboWorldRoot);
|
|
const editorObjects = new THREE.Group();
|
|
sdfRoot.add(editorObjects);
|
|
const editorBoundaryObjects = new THREE.Group();
|
|
sdfRoot.add(editorBoundaryObjects);
|
|
const runtimeLayoutObjects = new THREE.Group();
|
|
gazeboWorldRoot.add(runtimeLayoutObjects);
|
|
const navigationMapRoot = new THREE.Group();
|
|
navigationMapRoot.name = 'navigation-occupancy-map';
|
|
sdfRoot.add(navigationMapRoot);
|
|
const pointCloudGeometry = SHOW_POINT_CLOUD ? new THREE.BufferGeometry() : null;
|
|
let pointCloudLayer = null;
|
|
let hasGlobalMapPointCloud = false;
|
|
// The live sensor point cloud belongs to the 3D view. 2D SLAM renders only
|
|
// LaserScan and must never poll the point-cloud endpoint.
|
|
const sensorPointCloudGeometry = SHOW_SENSOR_POINT_CLOUD ? new THREE.BufferGeometry() : null;
|
|
let sensorPointCloudLayer = null;
|
|
if (pointCloudGeometry) {
|
|
pointCloudLayer = new THREE.Points(
|
|
pointCloudGeometry,
|
|
new THREE.PointsMaterial({
|
|
color: 0x4fc3f7,
|
|
size: 0.035,
|
|
sizeAttenuation: true,
|
|
transparent: true,
|
|
opacity: 0.78,
|
|
depthWrite: false,
|
|
}),
|
|
);
|
|
pointCloudLayer.name = 'slam-point-cloud';
|
|
pointCloudLayer.renderOrder = 2;
|
|
sdfRoot.add(pointCloudLayer);
|
|
}
|
|
if (sensorPointCloudGeometry) {
|
|
sensorPointCloudLayer = new THREE.Points(
|
|
sensorPointCloudGeometry,
|
|
new THREE.PointsMaterial({
|
|
color: 0xff5c35,
|
|
size: 0.028,
|
|
sizeAttenuation: true,
|
|
transparent: true,
|
|
opacity: 0.82,
|
|
depthWrite: false,
|
|
}),
|
|
);
|
|
sensorPointCloudLayer.name = 'live-sensor-point-cloud';
|
|
sensorPointCloudLayer.renderOrder = 3;
|
|
sdfRoot.add(sensorPointCloudLayer);
|
|
}
|
|
const editorRaycaster = new THREE.Raycaster();
|
|
const editorTransform = EDITOR_MODE ? new TransformControls(camera, renderer.domElement) : null;
|
|
let editorLayout = { version: 1, objects: [] };
|
|
let editorSelectedId = null;
|
|
let editorSelectionBox = null;
|
|
let editorSelectionGlow = null;
|
|
|
|
function updateEditorSelection() {
|
|
if (!editorSelectionBox || !editorSelectionGlow || !editorTransform?.object) return;
|
|
editorSelectionBox.update();
|
|
const bounds = new THREE.Box3().setFromObject(editorTransform.object);
|
|
const size = bounds.getSize(new THREE.Vector3()).multiplyScalar(1.12);
|
|
editorSelectionGlow.position.copy(bounds.getCenter(new THREE.Vector3()));
|
|
editorSelectionGlow.scale.copy(size);
|
|
}
|
|
|
|
if (editorTransform) {
|
|
editorTransform.setSize(1.25);
|
|
editorTransform.setSpace('local');
|
|
editorTransform.showZ = false;
|
|
editorTransform.addEventListener('dragging-changed', (event) => {
|
|
controls.enabled = !event.value;
|
|
});
|
|
editorTransform.addEventListener('mouseUp', () => {
|
|
const mesh = editorTransform.object;
|
|
if (!mesh?.userData?.objectId) return;
|
|
const original = editorLayout.objects.find((item) => item.id === mesh.userData.objectId);
|
|
if (!original) return;
|
|
const next = {
|
|
position: [mesh.position.x, mesh.position.y, Math.max(0.05, original.size[2] * mesh.scale.z / 2)],
|
|
rotation: [0, 0, mesh.rotation.z],
|
|
size: [
|
|
Math.max(0.1, original.size[0] * mesh.scale.x),
|
|
Math.max(0.1, original.size[1] * mesh.scale.y),
|
|
Math.max(0.1, original.size[2] * mesh.scale.z),
|
|
],
|
|
};
|
|
window.parent.postMessage({ source:'autonomy-world-editor', type:'transform-object', id: original.id, patch: next }, '*');
|
|
});
|
|
editorTransform.addEventListener('change', updateEditorSelection);
|
|
// Three.js r181에서는 TransformControls의 시각 기즈모를 helper로 씬에 추가한다.
|
|
scene.add(editorTransform.getHelper());
|
|
}
|
|
|
|
function attachEditorTransform() {
|
|
if (!editorTransform) return;
|
|
const mesh = editorObjects.children.find((child) => child.userData?.objectId === editorSelectedId);
|
|
if (editorSelectionBox) {
|
|
scene.remove(editorSelectionBox);
|
|
editorSelectionBox.geometry?.dispose?.();
|
|
editorSelectionBox.material?.dispose?.();
|
|
editorSelectionBox = null;
|
|
}
|
|
if (editorSelectionGlow) {
|
|
scene.remove(editorSelectionGlow);
|
|
editorSelectionGlow.geometry?.dispose?.();
|
|
editorSelectionGlow.material?.dispose?.();
|
|
editorSelectionGlow = null;
|
|
}
|
|
if (!mesh) {
|
|
editorTransform.detach();
|
|
return;
|
|
}
|
|
editorTransform.attach(mesh);
|
|
editorSelectionBox = new THREE.BoxHelper(mesh, 0x28a7ff);
|
|
editorSelectionBox.material.transparent = true;
|
|
editorSelectionBox.material.opacity = 0.95;
|
|
editorSelectionBox.material.depthTest = false;
|
|
editorSelectionBox.renderOrder = 1000;
|
|
scene.add(editorSelectionBox);
|
|
editorSelectionGlow = new THREE.Mesh(
|
|
new THREE.BoxGeometry(1, 1, 1),
|
|
new THREE.MeshBasicMaterial({ color:0x1d9bf0, transparent:true, opacity:0.16, depthWrite:false, depthTest:false, side:THREE.BackSide })
|
|
);
|
|
editorSelectionGlow.renderOrder = 999;
|
|
scene.add(editorSelectionGlow);
|
|
updateEditorSelection();
|
|
}
|
|
|
|
function setEditorTransformMode(mode) {
|
|
if (!editorTransform) return;
|
|
editorTransform.setMode(mode === 'rotate' || mode === 'scale' ? mode : 'translate');
|
|
editorTransform.showX = mode !== 'rotate';
|
|
editorTransform.showY = mode !== 'rotate';
|
|
editorTransform.showZ = mode === 'rotate';
|
|
editorTransform.showXY = mode !== 'rotate';
|
|
editorTransform.showXZ = false;
|
|
editorTransform.showYZ = false;
|
|
if (mode === 'rotate') {
|
|
editorTransform.showX = false;
|
|
editorTransform.showY = false;
|
|
editorTransform.showZ = true;
|
|
}
|
|
}
|
|
|
|
if (EDITOR_MODE) setEditorTransformMode('translate');
|
|
|
|
function clearLayoutObjects(parent) {
|
|
while (parent.children.length) {
|
|
const child = parent.children.pop();
|
|
child.geometry?.dispose?.();
|
|
child.material?.dispose?.();
|
|
}
|
|
}
|
|
|
|
function createLayoutMesh(item, editor = false) {
|
|
const size = Array.isArray(item.size) ? item.size : [1, 1, 1];
|
|
const position = Array.isArray(item.position) ? item.position : [0, 0, 0.5];
|
|
const rotation = Array.isArray(item.rotation) ? item.rotation : [0, 0, 0];
|
|
const type = item.type === 'pillar' ? 'pillar' : item.type === 'wall' ? 'wall' : 'box';
|
|
const material = new THREE.MeshStandardMaterial({
|
|
color: editor
|
|
? (type === 'pillar' ? 0xfb923c : type === 'wall' ? 0x60a5fa : 0xa78bfa)
|
|
: (type === 'pillar' ? 0x8aa6c6 : type === 'wall' ? 0x74869a : 0x919aa6),
|
|
roughness: 0.68,
|
|
side: THREE.DoubleSide,
|
|
});
|
|
const geometry = type === 'pillar'
|
|
? new THREE.CylinderGeometry(Math.max(size[0], size[1]) / 2, Math.max(size[0], size[1]) / 2, size[2], 32)
|
|
: new THREE.BoxGeometry(size[0], size[1], size[2]);
|
|
const mesh = new THREE.Mesh(geometry, material);
|
|
mesh.position.set(position[0], position[1], position[2]);
|
|
mesh.rotation.set(rotation[0], rotation[1], rotation[2]);
|
|
if (type === 'pillar') mesh.rotateX(Math.PI / 2);
|
|
mesh.castShadow = true;
|
|
mesh.receiveShadow = true;
|
|
mesh.userData.objectId = item.id;
|
|
return mesh;
|
|
}
|
|
|
|
function renderEditorLayout(layout) {
|
|
if (!EDITOR_MODE) return;
|
|
editorLayout = layout && Array.isArray(layout.objects) ? layout : { version:1, bounds:{ width:10, depth:10 }, objects:[] };
|
|
clearLayoutObjects(editorObjects);
|
|
clearLayoutObjects(editorBoundaryObjects);
|
|
for (const item of editorLayout.objects) {
|
|
editorObjects.add(createLayoutMesh(item, true));
|
|
}
|
|
attachEditorTransform();
|
|
}
|
|
|
|
let runtimeLayoutSignature = '';
|
|
async function refreshRuntimeLayout() {
|
|
if (EDITOR_MODE || REAL_RUNTIME || !CUSTOM_WORLD) return;
|
|
try {
|
|
const layout = await fetch(`${BACKEND_URL}/world-layout`).then(response => response.ok ? response.json() : { objects: [] });
|
|
const signature = JSON.stringify({ bounds: layout?.bounds || null, objects: layout?.objects || [] });
|
|
if (signature === runtimeLayoutSignature) return;
|
|
runtimeLayoutSignature = signature;
|
|
clearLayoutObjects(runtimeLayoutObjects);
|
|
for (const item of layout?.objects || []) runtimeLayoutObjects.add(createLayoutMesh(item));
|
|
} catch (_) {
|
|
// The visualization remains available while the Map module is offline.
|
|
}
|
|
}
|
|
|
|
window.addEventListener('message', (event) => {
|
|
if (!EDITOR_MODE || event.data?.source !== 'autonomy-world-editor') return;
|
|
if (event.data.type === 'set-layout') {
|
|
editorSelectedId = typeof event.data.selectedId === 'string' ? event.data.selectedId : null;
|
|
renderEditorLayout(event.data.layout);
|
|
}
|
|
if (event.data.type === 'set-transform-mode') setEditorTransformMode(event.data.mode);
|
|
});
|
|
|
|
const pendingSimulatorPoseRequests = new Map();
|
|
window.addEventListener('message', (event) => {
|
|
const payload = event.data;
|
|
if (payload?.source !== 'autonomy-studio' || payload.type !== 'simulator-pose-result') return;
|
|
const request = pendingSimulatorPoseRequests.get(payload.requestId);
|
|
if (!request) return;
|
|
pendingSimulatorPoseRequests.delete(payload.requestId);
|
|
window.clearTimeout(request.timeout);
|
|
if (payload.ok) request.resolve();
|
|
else request.reject(new Error(payload.message || '가제보 로봇 위치를 적용하지 못했습니다.'));
|
|
});
|
|
|
|
let robotModel = null;
|
|
let posePreviewModel = null;
|
|
let mkMiniFrontWheelPivots = [];
|
|
let targetMkMiniSteeringAngle = 0;
|
|
let currentMkMiniSteeringAngle = 0;
|
|
let targetMkMiniSteeringAngles = { left: 0, right: 0 };
|
|
let currentMkMiniSteeringAngles = { left: 0, right: 0 };
|
|
let lastMkMiniSteeringUpdateMs = null;
|
|
let lastMkMiniMotionSample = null;
|
|
let hasSimulatorMkMiniSteering = false;
|
|
let laserGroup = null;
|
|
let laserFillMesh = null;
|
|
let laserLineSegments = null;
|
|
let laserOpenSpaceLineSegments = null;
|
|
let laserFrontBeam = null;
|
|
let slamSocket = null;
|
|
let controlSocket = null;
|
|
let latestSlamState = null;
|
|
let currentRobotPose = { x:-2.0, y:-0.5, z:0.01, roll:0, pitch:0, yaw:0 };
|
|
let targetRobotPose = { ...currentRobotPose };
|
|
let hasRobotPose = false;
|
|
let latestMiniMapGrid = null;
|
|
let miniMapBitmap = null;
|
|
let navigationMapMesh = null;
|
|
let navigationMapTexture = null;
|
|
let navigationMapLocked = false;
|
|
let navigationMapFocused = false;
|
|
let latestScanMetadata = null;
|
|
let latestScanRanges = null;
|
|
let lastLaserRenderAtMs = 0;
|
|
let lastMiniMapRenderAtMs = 0;
|
|
let pendingMiniMapState = null;
|
|
let miniMapRenderTimer = null;
|
|
let pointCloudRequestInFlight = false;
|
|
let pointCloudRefreshTimer = null;
|
|
let savedMapPointCloudLoaded = false;
|
|
let sensorPointCloudRequestInFlight = false;
|
|
let sensorPointCloudRefreshTimer = null;
|
|
let cameraFrameRequestInFlight = false;
|
|
let cameraFrameRefreshTimer = null;
|
|
let cameraFrameObjectUrl = null;
|
|
let pathVisible = false;
|
|
// SLAM always renders its live scan, including the forward beam. Navigation
|
|
// keeps the sensor opt-in so a saved-map view starts cleanly.
|
|
let navigationSensorVisible = NAV2_VIEW_MODE !== 'navigation';
|
|
if (sensorPointCloudLayer) sensorPointCloudLayer.visible = navigationSensorVisible;
|
|
sensorToggleButton.classList.toggle('active', navigationSensorVisible);
|
|
sensorToggleButton.setAttribute('aria-pressed', String(navigationSensorVisible));
|
|
const robotPathPoints = [];
|
|
let robotPathLine = null;
|
|
let navigationPlanLine = null;
|
|
let navigationPlanSignature = '';
|
|
let navigationPlanSuppressed = NAV2_VIEW_MODE === 'navigation';
|
|
const runtimeObstacleObjects = new THREE.Group();
|
|
runtimeObstacleObjects.name = 'runtime-obstacles';
|
|
sdfRoot.add(runtimeObstacleObjects);
|
|
let activeCameraPreset = 'default';
|
|
let navigationGoalMode = false;
|
|
let initialPoseMode = false;
|
|
let initialPoseDrag = null;
|
|
let initialPoseDirectionArrow = null;
|
|
let obstaclePlacementMode = false;
|
|
let navigationInitialPoseSet = NAV2_VIEW_MODE !== 'navigation';
|
|
let navigationLocalizationReady = NAV2_VIEW_MODE !== 'navigation';
|
|
let navigationMapToOdom = null;
|
|
let navigationGoalMarker = null;
|
|
let navigationGoalId = null;
|
|
let navigationGoalPose = null;
|
|
let navigationGoalPreview = null;
|
|
let obstaclePreview = null;
|
|
let obstaclePreviewTemplate = null;
|
|
const navigationRaycaster = new THREE.Raycaster();
|
|
const navigationGroundPlane = new THREE.Plane(new THREE.Vector3(0, 1, 0), 0);
|
|
|
|
// A saved map, the remote room model, and AMCL's pose all use `map`.
|
|
// Keep every navigation scene in that frame. Rendering a local Gazebo session
|
|
// in `odom` made AMCL's map -> odom correction rotate and translate the map
|
|
// itself whenever an initial pose was set.
|
|
function navigationSceneUsesMapFrame() {
|
|
return NAV2_VIEW_MODE === 'navigation';
|
|
}
|
|
|
|
function localGazeboNavigationUsesOdom() {
|
|
return !REAL_RUNTIME && NAV2_VIEW_MODE === 'navigation';
|
|
}
|
|
|
|
function shouldRenderRobot() {
|
|
return hasRobotPose && (NAV2_VIEW_MODE !== 'navigation' || navigationInitialPoseSet);
|
|
}
|
|
|
|
function hasNavigationLocalization(state) {
|
|
const mapPose = state?.pose_3d || state?.pose_map;
|
|
return Boolean(mapPose && state?.tf?.map_to_odom);
|
|
}
|
|
|
|
function syncGoalToggleAvailability() {
|
|
const navigationReady = navigationInitialPoseSet || navigationLocalizationReady;
|
|
const goalDisabled = NAV2_VIEW_MODE === 'navigation' && !navigationReady;
|
|
const poseDisabled = NAV2_VIEW_MODE === 'navigation' && !REAL_RUNTIME && navigationReady;
|
|
goalToggleButton.disabled = goalDisabled;
|
|
poseToggleButton.disabled = poseDisabled;
|
|
poseToggleButton.title = poseDisabled
|
|
? '주행 시뮬레이션에서는 초기 위치를 한 번만 설정할 수 있습니다.'
|
|
: '로봇 초기 위치 설정';
|
|
if (goalDisabled) {
|
|
navigationGoalMode = false;
|
|
goalToggleButton.classList.remove('active');
|
|
hideNavigationGoalPreview();
|
|
}
|
|
if (poseDisabled) {
|
|
initialPoseMode = false;
|
|
poseToggleButton.classList.remove('active');
|
|
hideInitialPosePreview();
|
|
}
|
|
}
|
|
|
|
function updateNavigationLocalizationState(state) {
|
|
if (NAV2_VIEW_MODE !== 'navigation') return;
|
|
navigationLocalizationReady = Boolean(
|
|
hasNavigationLocalization(state)
|
|
&& state?.topic_status?.odom?.available === true
|
|
&& state?.topic_status?.map?.available === true
|
|
);
|
|
if (navigationLocalizationReady) navigationPlanSuppressed = false;
|
|
syncGoalToggleAvailability();
|
|
}
|
|
|
|
function restoreNavigationPoseFromState(state) {
|
|
if (NAV2_VIEW_MODE !== 'navigation' || navigationInitialPoseSet) return;
|
|
if (state?.initial_pose_set !== true) return;
|
|
if (!hasNavigationLocalization(state)) return;
|
|
|
|
const restoredPose = state?.pose_3d || state?.pose_map || state?.pose;
|
|
if (!restoredPose) return;
|
|
|
|
// The bridge remains alive when only the viewer is minimized or closed.
|
|
// Reuse its current ROS pose instead of making the user initialize again.
|
|
navigationInitialPoseSet = true;
|
|
navigationPlanSuppressed = false;
|
|
syncGoalToggleAvailability();
|
|
if (robotModel) robotModel.visible = true;
|
|
}
|
|
|
|
function resetNavigationPoseForNewSession(state) {
|
|
if (NAV2_VIEW_MODE !== 'navigation' || state?.initial_pose_set !== false || !navigationInitialPoseSet) return;
|
|
|
|
navigationInitialPoseSet = false;
|
|
navigationPlanSuppressed = true;
|
|
hasRobotPose = false;
|
|
clearNavigationGoalMarker();
|
|
navigationGoalId = null;
|
|
hideInitialPosePreview();
|
|
clearLaserScan();
|
|
if (robotModel) robotModel.visible = false;
|
|
syncGoalToggleAvailability();
|
|
}
|
|
|
|
syncGoalToggleAvailability();
|
|
|
|
function addWorldAxis(length = 1000) {
|
|
const axis = new THREE.Group();
|
|
axis.position.z = 0.02;
|
|
const axes = [
|
|
{ color:0xff0000, points:[[0,0,0], [length,0,0]] },
|
|
{ color:0x00aa00, points:[[0,0,0], [0,length,0]] },
|
|
{ color:0x0066ff, points:[[0,0,0], [0,0,length]] },
|
|
];
|
|
for (const item of axes) {
|
|
const geo = new THREE.BufferGeometry().setFromPoints(
|
|
item.points.map(([x, y, z]) => new THREE.Vector3(x, y, z))
|
|
);
|
|
axis.add(new THREE.Line(geo, new THREE.LineBasicMaterial({ color:item.color })));
|
|
}
|
|
sdfRoot.add(axis);
|
|
}
|
|
addWorldAxis();
|
|
|
|
function parseNums(text, count = 0) {
|
|
const nums = (text || '').trim().split(/\s+/).filter(Boolean).map(Number).filter(n => !Number.isNaN(n));
|
|
while (nums.length < count) nums.push(0);
|
|
return nums;
|
|
}
|
|
function parsePose(text) {
|
|
const v = parseNums(text, 6);
|
|
return { x:v[0], y:v[1], z:v[2], roll:v[3], pitch:v[4], yaw:v[5] };
|
|
}
|
|
function applySdfPose(obj, pose) {
|
|
obj.position.set(pose.x, pose.y, pose.z);
|
|
obj.rotation.set(pose.roll, pose.pitch, pose.yaw, 'XYZ');
|
|
}
|
|
function applyWorldSdfPose(obj, pose) {
|
|
applySdfPose(obj, pose);
|
|
}
|
|
function isMkMiniWorldPillar(visual) {
|
|
return MKMINI_WORLD_SINGLE_PILLAR
|
|
&& /^(one|two|three)_/.test(visual.getAttribute('name') || '');
|
|
}
|
|
function shouldRenderWorldVisual(visual) {
|
|
// The MK-mini Gazebo world keeps only the centre post from the 3x3 pillar
|
|
// grid. Mirror that generated-world rule in the Three.js preview.
|
|
return !isMkMiniWorldPillar(visual) || visual.getAttribute('name') === 'two_two';
|
|
}
|
|
function getFirst(el, tag) { return el?.getElementsByTagName(tag)?.[0] || null; }
|
|
function directChild(el, tag) { return Array.from(el.children || []).find(c => c.tagName === tag) || null; }
|
|
function getDirectText(el, tag, fallback = '') {
|
|
const n = directChild(el, tag);
|
|
return n?.textContent?.trim() || fallback;
|
|
}
|
|
const MODEL_ROOTS = {
|
|
turtlebot3_world: './turtlebot3_world_model/turtlebot3_world',
|
|
turtlebot3_house: './turtlebot3_world_model/turtlebot3_house',
|
|
turtlebot3_common: './turtlebot3_world_model/turtlebot3_common',
|
|
mailbox: './turtlebot3_world_model/mailbox',
|
|
};
|
|
function gazeboUriToPath(uri) {
|
|
return uri.replace(/^model:\/\/(\w+)\//, (_, name) =>
|
|
(MODEL_ROOTS[name] ?? `./turtlebot3_world_model/${name}`) + '/'
|
|
);
|
|
}
|
|
function parseScale(text) {
|
|
const s = parseNums(text, 3);
|
|
if (s.length >= 3 && s.every(n => Number.isFinite(n))) return new THREE.Vector3(s[0], s[1], s[2]);
|
|
return new THREE.Vector3(1,1,1);
|
|
}
|
|
function materialFromVisual(visual) {
|
|
let color = 0xdddddd;
|
|
const visualName = visual.getAttribute('name') || '';
|
|
const uriText = Array.from(visual.getElementsByTagName('uri')).map(n => n.textContent.trim()).join(' ');
|
|
const scriptName = Array.from(visual.getElementsByTagName('script')).map(s => getDirectText(s, 'name', '')).join(' ');
|
|
const combined = `${visualName} ${uriText} ${scriptName}`.toLowerCase();
|
|
if (combined.includes('wall')) color = 0x080808;
|
|
else if (combined.includes('hexagon') || combined.includes('green')) color = 0x00a000;
|
|
else if (combined.includes('cylinder') || combined.includes('white')) color = 0xf0f0f0;
|
|
|
|
const ambientNode = visual.getElementsByTagName('ambient')[0];
|
|
const ambient = ambientNode?.textContent?.trim();
|
|
if (ambient) {
|
|
const c = parseNums(ambient);
|
|
if (c.length >= 3) color = new THREE.Color(c[0], c[1], c[2]);
|
|
}
|
|
return new THREE.MeshLambertMaterial({ color });
|
|
}
|
|
function prepMesh(mesh, material) {
|
|
mesh.traverse(child => {
|
|
if (child.isMesh) {
|
|
child.material = material.clone();
|
|
child.castShadow = true;
|
|
child.receiveShadow = true;
|
|
}
|
|
});
|
|
}
|
|
|
|
const colladaLoader = new ColladaLoader();
|
|
if (colladaLoader.options) colladaLoader.options.convertUpAxis = false;
|
|
const stlLoader = new STLLoader();
|
|
|
|
async function addMesh(parent, visual, geometry) {
|
|
const meshEl = directChild(geometry, 'mesh');
|
|
const uri = getDirectText(meshEl, 'uri');
|
|
if (!uri) return;
|
|
const url = gazeboUriToPath(uri);
|
|
const scale = parseScale(getDirectText(meshEl, 'scale', '1 1 1'));
|
|
const material = materialFromVisual(visual);
|
|
|
|
if (/\.stl$/i.test(url)) {
|
|
await new Promise(resolve => {
|
|
stlLoader.load(url, geo => {
|
|
geo.computeVertexNormals();
|
|
const mesh = new THREE.Mesh(geo, material);
|
|
mesh.scale.copy(scale);
|
|
mesh.castShadow = true;
|
|
mesh.receiveShadow = true;
|
|
parent.add(mesh);
|
|
resolve();
|
|
}, undefined, err => {
|
|
console.warn('STL load failed:', url, err);
|
|
parent.add(new THREE.Mesh(new THREE.BoxGeometry(.1,.1,.1), new THREE.MeshLambertMaterial({ color:0xff5555 })));
|
|
resolve();
|
|
});
|
|
});
|
|
} else {
|
|
await new Promise(resolve => {
|
|
colladaLoader.load(url, collada => {
|
|
const obj = collada.scene;
|
|
obj.scale.multiply(scale);
|
|
// mailbox.dae는 Z_UP인데 SDF 루트도 Z-up -> Y-up 변환을 한다.
|
|
// ColladaLoader의 변환을 한 번 상쇄해 Gazebo와 같은 방향으로 둔다.
|
|
if (uri.startsWith('model://mailbox/')) obj.rotation.x += Math.PI / 2;
|
|
prepMesh(obj, material);
|
|
parent.add(obj);
|
|
resolve();
|
|
}, undefined, err => {
|
|
console.warn('DAE load failed:', url, err);
|
|
parent.add(new THREE.Mesh(new THREE.BoxGeometry(.4,.4,.4), new THREE.MeshLambertMaterial({ color:0xff5555 })));
|
|
resolve();
|
|
});
|
|
});
|
|
}
|
|
}
|
|
function addBox(parent, visual, geometry) {
|
|
const box = directChild(geometry, 'box');
|
|
const s = parseNums(getDirectText(box, 'size', '1 1 1'), 3);
|
|
const mesh = new THREE.Mesh(new THREE.BoxGeometry(s[0], s[1], s[2]), materialFromVisual(visual));
|
|
mesh.castShadow = true; mesh.receiveShadow = true;
|
|
parent.add(mesh);
|
|
}
|
|
function addCylinder(parent, visual, geometry) {
|
|
const cyl = directChild(geometry, 'cylinder');
|
|
const radius = Number(getDirectText(cyl, 'radius', '0.1')) || .1;
|
|
const sourceLength = Number(getDirectText(cyl, 'length', '1')) || 1;
|
|
const length = isMkMiniWorldPillar(visual) && sourceLength === .5 ? 1.2 : sourceLength;
|
|
const mesh = new THREE.Mesh(new THREE.CylinderGeometry(radius, radius, length, 32), materialFromVisual(visual));
|
|
// Three cylinder 기본 축은 Y, SDF cylinder 기본 축은 Z입니다.
|
|
mesh.rotation.x = Math.PI / 2;
|
|
mesh.castShadow = true; mesh.receiveShadow = true;
|
|
parent.add(mesh);
|
|
}
|
|
function addSphere(parent, visual, geometry) {
|
|
const sph = directChild(geometry, 'sphere');
|
|
const radius = Number(getDirectText(sph, 'radius', '0.1')) || .1;
|
|
const mesh = new THREE.Mesh(new THREE.SphereGeometry(radius, 32, 16), materialFromVisual(visual));
|
|
mesh.castShadow = true; mesh.receiveShadow = true;
|
|
parent.add(mesh);
|
|
}
|
|
|
|
async function buildFromSdf() {
|
|
const xmlText = await fetch(MODEL_SDF).then(r => {
|
|
if (!r.ok) throw new Error(`${MODEL_SDF} load failed: ${r.status}`);
|
|
return r.text();
|
|
});
|
|
const doc = new DOMParser().parseFromString(xmlText, 'application/xml');
|
|
if (doc.querySelector('parsererror')) throw new Error(doc.querySelector('parsererror').textContent);
|
|
|
|
const model = doc.querySelector('model');
|
|
const modelGroup = new THREE.Group();
|
|
modelGroup.name = model?.getAttribute('name') || 'turtlebot3_world';
|
|
applyWorldSdfPose(modelGroup, parsePose(getDirectText(model, 'pose', '0 0 0 0 0 0')));
|
|
gazeboWorldRoot.add(modelGroup);
|
|
|
|
let loaded = 0;
|
|
async function addModelVisuals(sdfModel, parent) {
|
|
const nestedModelGroup = new THREE.Group();
|
|
let directGeometryCount = 0;
|
|
nestedModelGroup.name = sdfModel.getAttribute('name') || 'model';
|
|
applyWorldSdfPose(nestedModelGroup, parsePose(getDirectText(sdfModel, 'pose', '0 0 0 0 0 0')));
|
|
parent.add(nestedModelGroup);
|
|
|
|
for (const link of Array.from(sdfModel.querySelectorAll(':scope > link'))) {
|
|
const linkGroup = new THREE.Group();
|
|
linkGroup.name = link.getAttribute('name') || 'link';
|
|
applyWorldSdfPose(linkGroup, parsePose(getDirectText(link, 'pose', '0 0 0 0 0 0')));
|
|
nestedModelGroup.add(linkGroup);
|
|
|
|
for (const visual of Array.from(link.querySelectorAll(':scope > visual'))) {
|
|
if (!shouldRenderWorldVisual(visual)) continue;
|
|
const visualGroup = new THREE.Group();
|
|
visualGroup.name = visual.getAttribute('name') || 'visual';
|
|
applyWorldSdfPose(visualGroup, parsePose(getDirectText(visual, 'pose', '0 0 0 0 0 0')));
|
|
if (isMkMiniWorldPillar(visual)) visualGroup.position.z = 0.6;
|
|
linkGroup.add(visualGroup);
|
|
|
|
const geometry = directChild(visual, 'geometry');
|
|
if (!geometry) continue;
|
|
if (directChild(geometry, 'mesh')) {
|
|
const uri = getDirectText(directChild(geometry, 'mesh'), 'uri');
|
|
// 번들에 포함한 메시만 읽고, 재질은 단색으로 덮어쓴다.
|
|
// 나머지 Gazebo 모델은 아래의 단순 형상으로 표시한다.
|
|
if (/^model:\/\/(turtlebot3_world|turtlebot3_house|turtlebot3_common|turtlebot3_burger|mailbox)\//.test(uri)) {
|
|
await addMesh(visualGroup, visual, geometry);
|
|
loaded++;
|
|
directGeometryCount++;
|
|
}
|
|
} else if (directChild(geometry, 'box')) {
|
|
addBox(visualGroup, visual, geometry);
|
|
loaded++;
|
|
directGeometryCount++;
|
|
} else if (directChild(geometry, 'cylinder')) {
|
|
addCylinder(visualGroup, visual, geometry);
|
|
loaded++;
|
|
directGeometryCount++;
|
|
} else if (directChild(geometry, 'sphere')) {
|
|
addSphere(visualGroup, visual, geometry);
|
|
loaded++;
|
|
directGeometryCount++;
|
|
}
|
|
}
|
|
}
|
|
|
|
for (const childModel of Array.from(sdfModel.querySelectorAll(':scope > model'))) {
|
|
await addModelVisuals(childModel, nestedModelGroup);
|
|
}
|
|
|
|
// House 월드의 mailbox, trash can, marble table 등은 Gazebo 기본
|
|
// 메시 파일만 참조합니다. 시각화에는 무거운 메시/텍스처 대신 위치와
|
|
// 점유 영역을 나타내는 간단한 형상을 둡니다.
|
|
const modelName = (sdfModel.getAttribute('name') || '').toLowerCase();
|
|
if (sdfModel !== model && directGeometryCount === 0 && modelName) {
|
|
const material = new THREE.MeshLambertMaterial({ color: modelName.includes('trash') ? 0x64748b : 0x9a7b5d });
|
|
let proxy;
|
|
if (modelName.includes('trash')) {
|
|
proxy = new THREE.Mesh(new THREE.CylinderGeometry(0.24, 0.2, 0.72, 20), material);
|
|
proxy.rotation.x = Math.PI / 2;
|
|
} else if (modelName.includes('mailbox')) {
|
|
proxy = new THREE.Mesh(new THREE.BoxGeometry(0.45, 0.28, 0.36), material);
|
|
proxy.position.z = 0.18;
|
|
} else {
|
|
proxy = new THREE.Mesh(new THREE.BoxGeometry(1.45, 0.85, 0.74), material);
|
|
proxy.position.z = 0.37;
|
|
}
|
|
proxy.castShadow = true;
|
|
proxy.receiveShadow = true;
|
|
nestedModelGroup.add(proxy);
|
|
loaded++;
|
|
}
|
|
}
|
|
|
|
// House 월드는 벽 외에도 책장, 캐비닛, 테이블을 하위 model로 정의합니다.
|
|
// 모델 계층을 함께 읽어 Gazebo와 동일한 장애물 배치를 단색 형상으로 그립니다.
|
|
await addModelVisuals(model, modelGroup);
|
|
|
|
if (!loaded) console.warn(`no visuals loaded from ${MODEL_SDF}`);
|
|
}
|
|
|
|
async function loadBurgerModel(sdfPose = { x:0, y:0, z:0, roll:0, pitch:0, yaw:0 }) {
|
|
const BURGER_SDF = './turtlebot3_world_model/turtlebot3_burger/model.sdf';
|
|
const xmlText = await fetch(BURGER_SDF).then(r => {
|
|
if (!r.ok) throw new Error(`burger SDF load failed: ${r.status}`);
|
|
return r.text();
|
|
});
|
|
const doc = new DOMParser().parseFromString(xmlText, 'application/xml');
|
|
|
|
const model = doc.querySelector('model');
|
|
const robotGroup = new THREE.Group();
|
|
robotGroup.name = 'turtlebot3_burger';
|
|
applySdfPose(robotGroup, sdfPose);
|
|
|
|
for (const link of Array.from(model.querySelectorAll(':scope > link'))) {
|
|
const linkGroup = new THREE.Group();
|
|
linkGroup.name = link.getAttribute('name') || 'link';
|
|
applySdfPose(linkGroup, parsePose(getDirectText(link, 'pose', '0 0 0 0 0 0')));
|
|
robotGroup.add(linkGroup);
|
|
|
|
for (const visual of Array.from(link.querySelectorAll(':scope > visual'))) {
|
|
const vg = new THREE.Group();
|
|
vg.name = visual.getAttribute('name') || 'visual';
|
|
applySdfPose(vg, parsePose(getDirectText(visual, 'pose', '0 0 0 0 0 0')));
|
|
linkGroup.add(vg);
|
|
|
|
const geometry = directChild(visual, 'geometry');
|
|
if (!geometry) continue;
|
|
if (directChild(geometry, 'mesh')) await addMesh(vg, visual, geometry);
|
|
else if (directChild(geometry, 'cylinder')) addCylinder(vg, visual, geometry);
|
|
}
|
|
}
|
|
|
|
sdfRoot.add(robotGroup);
|
|
robotModel = robotGroup;
|
|
robotModel.visible = shouldRenderRobot();
|
|
|
|
posePreviewModel = robotGroup.clone(true);
|
|
posePreviewModel.name = 'turtlebot3_burger_pose_preview';
|
|
posePreviewModel.traverse((child) => {
|
|
if (!child.isMesh) return;
|
|
if (Array.isArray(child.material)) {
|
|
child.material = child.material.map((material) => material.clone());
|
|
child.material.forEach((material) => {
|
|
material.transparent = true;
|
|
material.opacity = 0.38;
|
|
material.depthWrite = false;
|
|
});
|
|
return;
|
|
}
|
|
child.material = child.material.clone();
|
|
child.material.transparent = true;
|
|
child.material.opacity = 0.38;
|
|
child.material.depthWrite = false;
|
|
});
|
|
posePreviewModel.visible = false;
|
|
sdfRoot.add(posePreviewModel);
|
|
}
|
|
|
|
async function loadMkMiniModel(sdfPose = { x:0, y:0, z:0, roll:0, pitch:0, yaw:0 }) {
|
|
const gltf = await new GLTFLoader().loadAsync('./assets/mk-mini.glb');
|
|
const robotGroup = new THREE.Group();
|
|
robotGroup.name = 'mkmini';
|
|
applySdfPose(robotGroup, sdfPose);
|
|
|
|
const model = gltf.scene;
|
|
// The source asset is Z-up; convert it to the viewer's Y-up coordinate system.
|
|
model.rotation.x = Math.PI / 2;
|
|
model.updateMatrixWorld(true);
|
|
const initialBounds = new THREE.Box3().setFromObject(model);
|
|
const initialSize = initialBounds.getSize(new THREE.Vector3());
|
|
// The GLB is rotated into the SDF coordinate system above. Its local axes
|
|
// then map to SDF as X=length, Z=width, Y=height. Normalize every axis to
|
|
// the same physical envelope used by the Gazebo collision model.
|
|
const scaleX = MKMINI_RENDER_LENGTH / Math.max(initialSize.x, 0.001);
|
|
const scaleY = MKMINI_RENDER_HEIGHT / Math.max(initialSize.z, 0.001);
|
|
const scaleZ = MKMINI_RENDER_WIDTH / Math.max(initialSize.y, 0.001);
|
|
model.scale.set(scaleX, scaleY, scaleZ);
|
|
model.updateMatrixWorld(true);
|
|
|
|
const normalizedBounds = new THREE.Box3().setFromObject(model);
|
|
const center = normalizedBounds.getCenter(new THREE.Vector3());
|
|
// Keep the wheels on SDF Z=0 after scaling the vehicle height.
|
|
model.position.set(-center.x, -center.y, -normalizedBounds.min.z);
|
|
model.traverse((child) => {
|
|
if (!child.isMesh) return;
|
|
child.castShadow = true;
|
|
child.receiveShadow = true;
|
|
});
|
|
mkMiniFrontWheelPivots = createMkMiniFrontWheelPivots(model);
|
|
robotGroup.add(model);
|
|
sdfRoot.add(robotGroup);
|
|
robotModel = robotGroup;
|
|
robotModel.visible = shouldRenderRobot();
|
|
|
|
posePreviewModel = robotGroup.clone(true);
|
|
posePreviewModel.name = 'mkmini_pose_preview';
|
|
posePreviewModel.traverse((child) => {
|
|
if (!child.isMesh) return;
|
|
child.material = Array.isArray(child.material)
|
|
? child.material.map((material) => material.clone())
|
|
: child.material.clone();
|
|
const materials = Array.isArray(child.material) ? child.material : [child.material];
|
|
materials.forEach((material) => {
|
|
material.transparent = true;
|
|
material.opacity = 0.38;
|
|
material.depthWrite = false;
|
|
});
|
|
});
|
|
posePreviewModel.visible = false;
|
|
sdfRoot.add(posePreviewModel);
|
|
}
|
|
|
|
function createMkMiniFrontWheelPivots(model) {
|
|
const pivots = [];
|
|
model.updateMatrixWorld(true);
|
|
for (const [wheelName, side] of [['FrontWheel_L', 'left'], ['FrontWheel_R', 'right']]) {
|
|
const wheel = model.getObjectByName(wheelName);
|
|
if (!wheel) continue;
|
|
|
|
const bounds = new THREE.Box3().setFromObject(wheel);
|
|
const center = bounds.getCenter(new THREE.Vector3());
|
|
const pivot = new THREE.Group();
|
|
pivot.name = `${wheelName}_steering_pivot`;
|
|
model.add(pivot);
|
|
pivot.position.copy(model.worldToLocal(center));
|
|
pivot.updateMatrixWorld(true);
|
|
// The source GLB keeps the wheel vertices in model coordinates. Reparenting
|
|
// through a local pivot preserves their pose while giving each wheel a steer axis.
|
|
pivot.attach(wheel);
|
|
pivots.push({ pivot, side });
|
|
}
|
|
return pivots;
|
|
}
|
|
|
|
const LASER_HOLE_R = 0.14;
|
|
const LASER_RENDER_INTERVAL_MS = 100;
|
|
const LASER_OPEN_SPACE_RAY_STRIDE = 4;
|
|
const MINI_MAP_RENDER_INTERVAL_MS = 100;
|
|
const LASER_OFFSET_X = USE_MKMINI_MODEL ? 0.15 : -0.032;
|
|
const LASER_OFFSET_Y = 0;
|
|
// Fallback height when an explicit ROS scan pose is unavailable.
|
|
const LASER_Z = USE_MKMINI_MODEL ? 0.5 : 0.181;
|
|
const MKMINI_LASER_RENDER_BACKWARD_OFFSET = 0.03;
|
|
|
|
function setRobotPose(pose) {
|
|
if (!pose) return;
|
|
targetRobotPose = {
|
|
x: Number(pose.x ?? targetRobotPose.x),
|
|
y: Number(pose.y ?? targetRobotPose.y),
|
|
// Gazebo navigation is a ground-plane simulation. A saved 3D map can
|
|
// include an elevated RTAB-Map pose, but that must not lift the rendered
|
|
// vehicle after the learner supplies a 2D initial pose.
|
|
z: REAL_RUNTIME ? Number(pose.z ?? targetRobotPose.z) : 0,
|
|
roll: REAL_RUNTIME ? Number(pose.roll ?? targetRobotPose.roll) : 0,
|
|
pitch: REAL_RUNTIME ? Number(pose.pitch ?? targetRobotPose.pitch) : 0,
|
|
yaw: Number(pose.yaw ?? targetRobotPose.yaw),
|
|
};
|
|
updateMkMiniSteeringTarget(pose);
|
|
if (!hasRobotPose) {
|
|
currentRobotPose = { ...targetRobotPose };
|
|
hasRobotPose = true;
|
|
applyCurrentRobotPose();
|
|
} else if (activeMoveKey) {
|
|
// While teleoperating, render the latest ROS odometry immediately instead
|
|
// of letting interpolation accumulate behind the incoming state stream.
|
|
currentRobotPose = { ...targetRobotPose };
|
|
applyCurrentRobotPose();
|
|
updateRobotPath();
|
|
updateFollowCamera();
|
|
}
|
|
}
|
|
|
|
function applyInitialNavigationPose(pose) {
|
|
const x = Number(pose?.x);
|
|
const y = Number(pose?.y);
|
|
const yaw = Number(pose?.yaw ?? 0);
|
|
if (![x, y, yaw].every(Number.isFinite)) return;
|
|
|
|
// An initial pose is a teleport, not a movement. Snap the rendered robot to
|
|
// the selected map pose so stale odometry cannot animate it across the map
|
|
// while AMCL publishes the new map -> odom correction.
|
|
targetRobotPose = { ...targetRobotPose, x, y, z: 0, roll: 0, pitch: 0, yaw };
|
|
currentRobotPose = { ...targetRobotPose };
|
|
hasRobotPose = true;
|
|
lastMkMiniMotionSample = null;
|
|
applyCurrentRobotPose();
|
|
updateFollowCamera();
|
|
}
|
|
|
|
function lerpAngle(from, to, alpha) {
|
|
let delta = ((to - from + Math.PI) % (Math.PI * 2)) - Math.PI;
|
|
if (delta < -Math.PI) delta += Math.PI * 2;
|
|
return from + delta * alpha;
|
|
}
|
|
|
|
function applyCurrentRobotPose() {
|
|
if (!robotModel) return;
|
|
// Raw odometry is valid before localization, but it has no map-frame
|
|
// meaning. Keep the vehicle hidden until the learner sets the first pose.
|
|
robotModel.visible = shouldRenderRobot();
|
|
robotModel.position.set(currentRobotPose.x, currentRobotPose.y, currentRobotPose.z);
|
|
robotModel.rotation.set(currentRobotPose.roll, currentRobotPose.pitch, currentRobotPose.yaw, 'XYZ');
|
|
}
|
|
|
|
function updateMkMiniSteeringTarget(pose) {
|
|
if (!USE_MKMINI_MODEL || !mkMiniFrontWheelPivots.length) return;
|
|
|
|
const simulatorLeftSteeringAngle = Number(pose?.steeringLeftAngle);
|
|
const simulatorRightSteeringAngle = Number(pose?.steeringRightAngle);
|
|
if (Number.isFinite(simulatorLeftSteeringAngle) && Number.isFinite(simulatorRightSteeringAngle)) {
|
|
hasSimulatorMkMiniSteering = true;
|
|
targetMkMiniSteeringAngles = {
|
|
left: THREE.MathUtils.clamp(simulatorLeftSteeringAngle, -MKMINI_MAX_STEERING_RADIANS, MKMINI_MAX_STEERING_RADIANS),
|
|
right: THREE.MathUtils.clamp(simulatorRightSteeringAngle, -MKMINI_MAX_STEERING_RADIANS, MKMINI_MAX_STEERING_RADIANS),
|
|
};
|
|
targetMkMiniSteeringAngle = (targetMkMiniSteeringAngles.left + targetMkMiniSteeringAngles.right) * 0.5;
|
|
return;
|
|
}
|
|
|
|
const simulatorSteeringAngle = Number(pose?.steeringAngle);
|
|
if (Number.isFinite(simulatorSteeringAngle)) {
|
|
hasSimulatorMkMiniSteering = true;
|
|
targetMkMiniSteeringAngle = THREE.MathUtils.clamp(
|
|
simulatorSteeringAngle,
|
|
-MKMINI_MAX_STEERING_RADIANS,
|
|
MKMINI_MAX_STEERING_RADIANS,
|
|
);
|
|
targetMkMiniSteeringAngles = mkMiniAckermannAngles(targetMkMiniSteeringAngle);
|
|
return;
|
|
}
|
|
|
|
hasSimulatorMkMiniSteering = false;
|
|
|
|
const motion = pose?.motion ?? pose;
|
|
let linear = Number(motion?.linear);
|
|
let angular = Number(motion?.angular);
|
|
const sample = {
|
|
x: Number(motion?.x),
|
|
y: Number(motion?.y),
|
|
yaw: Number(motion?.yaw),
|
|
t: Number(motion?.t),
|
|
};
|
|
|
|
// Some vehicle odometry streams keep twist.angular stale. Derive the turn
|
|
// direction from consecutive odom poses when a fresh movement sample exists.
|
|
if (
|
|
lastMkMiniMotionSample
|
|
&& Number.isFinite(sample.x)
|
|
&& Number.isFinite(sample.y)
|
|
&& Number.isFinite(sample.yaw)
|
|
&& Number.isFinite(sample.t)
|
|
) {
|
|
const elapsed = sample.t - lastMkMiniMotionSample.t;
|
|
const deltaX = sample.x - lastMkMiniMotionSample.x;
|
|
const deltaY = sample.y - lastMkMiniMotionSample.y;
|
|
const distance = Math.hypot(deltaX, deltaY);
|
|
if (elapsed > 0.01 && elapsed < 1.5 && distance > 0.002) {
|
|
const forwardDistance = (
|
|
deltaX * Math.cos(lastMkMiniMotionSample.yaw)
|
|
+ deltaY * Math.sin(lastMkMiniMotionSample.yaw)
|
|
);
|
|
linear = forwardDistance / elapsed;
|
|
angular = lerpAngle(lastMkMiniMotionSample.yaw, sample.yaw, 1) - lastMkMiniMotionSample.yaw;
|
|
angular /= elapsed;
|
|
}
|
|
}
|
|
|
|
if (
|
|
Number.isFinite(sample.x)
|
|
&& Number.isFinite(sample.y)
|
|
&& Number.isFinite(sample.yaw)
|
|
&& Number.isFinite(sample.t)
|
|
) {
|
|
lastMkMiniMotionSample = sample;
|
|
}
|
|
|
|
if (!Number.isFinite(linear) || !Number.isFinite(angular)) return;
|
|
|
|
targetMkMiniSteeringAngle = Math.abs(linear) < 0.03
|
|
? 0
|
|
: THREE.MathUtils.clamp(
|
|
Math.atan2(MKMINI_WHEELBASE_METERS * angular, linear),
|
|
-MKMINI_MAX_STEERING_RADIANS,
|
|
MKMINI_MAX_STEERING_RADIANS,
|
|
);
|
|
targetMkMiniSteeringAngles = mkMiniAckermannAngles(targetMkMiniSteeringAngle);
|
|
}
|
|
|
|
function mkMiniAckermannAngles(steeringAngle) {
|
|
if (Math.abs(steeringAngle) < 0.0001) return { left: 0, right: 0 };
|
|
const direction = Math.sign(steeringAngle);
|
|
const radius = MKMINI_GAZEBO_WHEELBASE_METERS / Math.tan(Math.abs(steeringAngle));
|
|
const halfTrack = 0.255;
|
|
const inner = Math.atan(MKMINI_GAZEBO_WHEELBASE_METERS / Math.max(0.01, radius - halfTrack));
|
|
const outer = Math.atan(MKMINI_GAZEBO_WHEELBASE_METERS / (radius + halfTrack));
|
|
return direction > 0 ? { left: inner, right: outer } : { left: -outer, right: -inner };
|
|
}
|
|
|
|
function updateMkMiniFrontWheelSteering() {
|
|
if (!mkMiniFrontWheelPivots.length) return;
|
|
const now = performance.now();
|
|
const elapsedSeconds = lastMkMiniSteeringUpdateMs === null
|
|
? 0
|
|
: Math.min((now - lastMkMiniSteeringUpdateMs) / 1000, 0.1);
|
|
lastMkMiniSteeringUpdateMs = now;
|
|
const alpha = 1 - Math.exp(-8 * elapsedSeconds);
|
|
currentMkMiniSteeringAngle += (targetMkMiniSteeringAngle - currentMkMiniSteeringAngle) * alpha;
|
|
for (const { pivot, side } of mkMiniFrontWheelPivots) {
|
|
// The asset is Z-up before its model-level X rotation, so its local Y axis
|
|
// becomes the viewer's vertical steering axis.
|
|
currentMkMiniSteeringAngles[side] += (targetMkMiniSteeringAngles[side] - currentMkMiniSteeringAngles[side]) * alpha;
|
|
pivot.rotation.y = currentMkMiniSteeringAngles[side];
|
|
}
|
|
}
|
|
|
|
function updateSmoothedRobotPose() {
|
|
if (!hasRobotPose) return;
|
|
const alpha = 0.22;
|
|
currentRobotPose.x += (targetRobotPose.x - currentRobotPose.x) * alpha;
|
|
currentRobotPose.y += (targetRobotPose.y - currentRobotPose.y) * alpha;
|
|
currentRobotPose.z += (targetRobotPose.z - currentRobotPose.z) * alpha;
|
|
currentRobotPose.roll = lerpAngle(currentRobotPose.roll, targetRobotPose.roll, alpha);
|
|
currentRobotPose.pitch = lerpAngle(currentRobotPose.pitch, targetRobotPose.pitch, alpha);
|
|
currentRobotPose.yaw = lerpAngle(currentRobotPose.yaw, targetRobotPose.yaw, alpha);
|
|
applyCurrentRobotPose();
|
|
updateRobotPath();
|
|
updateFollowCamera();
|
|
}
|
|
|
|
function disposeObject(obj) {
|
|
obj.traverse(child => {
|
|
child.geometry?.dispose?.();
|
|
if (Array.isArray(child.material)) child.material.forEach(m => m.dispose?.());
|
|
else child.material?.dispose?.();
|
|
});
|
|
}
|
|
|
|
function updateRobotPath() {
|
|
const p = new THREE.Vector3(currentRobotPose.x, currentRobotPose.y, currentRobotPose.z + 0.03);
|
|
const last = robotPathPoints[robotPathPoints.length - 1];
|
|
let pathChanged = false;
|
|
if (!last || last.distanceTo(p) > 0.03) {
|
|
robotPathPoints.push(p);
|
|
if (robotPathPoints.length > 3000) robotPathPoints.shift();
|
|
pathChanged = true;
|
|
}
|
|
|
|
if (!pathVisible) return;
|
|
|
|
if (!robotPathLine) {
|
|
robotPathLine = new THREE.Line(
|
|
new THREE.BufferGeometry(),
|
|
new THREE.LineBasicMaterial({ color: 0xffcc00, linewidth: 2 })
|
|
);
|
|
sdfRoot.add(robotPathLine);
|
|
pathChanged = true;
|
|
}
|
|
|
|
if (!pathChanged) return;
|
|
|
|
robotPathLine.geometry.dispose();
|
|
robotPathLine.geometry = new THREE.BufferGeometry().setFromPoints(robotPathPoints);
|
|
}
|
|
|
|
function setPathVisible(visible) {
|
|
pathVisible = visible;
|
|
pathToggleButton.classList.toggle('active', visible);
|
|
if (!visible && robotPathLine) {
|
|
sdfRoot.remove(robotPathLine);
|
|
disposeObject(robotPathLine);
|
|
robotPathLine = null;
|
|
} else if (visible) {
|
|
updateRobotPath();
|
|
}
|
|
}
|
|
|
|
function clearRobotPath() {
|
|
robotPathPoints.length = 0;
|
|
if (!robotPathLine) return;
|
|
sdfRoot.remove(robotPathLine);
|
|
disposeObject(robotPathLine);
|
|
robotPathLine = null;
|
|
}
|
|
|
|
function laserOriginFromPose(pose) {
|
|
const q = new THREE.Quaternion().setFromEuler(new THREE.Euler(pose.roll || 0, pose.pitch || 0, pose.yaw || 0, 'XYZ'));
|
|
const offset = new THREE.Vector3(LASER_OFFSET_X, LASER_OFFSET_Y, LASER_Z).applyQuaternion(q);
|
|
return new THREE.Vector3(pose.x, pose.y, pose.z || 0).add(offset);
|
|
}
|
|
|
|
function laserDirectionFromPose(pose, angle) {
|
|
const q = new THREE.Quaternion().setFromEuler(new THREE.Euler(pose.roll || 0, pose.pitch || 0, pose.yaw || 0, 'XYZ'));
|
|
return new THREE.Vector3(Math.cos(angle), Math.sin(angle), 0).applyQuaternion(q).normalize();
|
|
}
|
|
|
|
function ensureLaserScanLayer() {
|
|
if (laserGroup) {
|
|
laserGroup.visible = true;
|
|
return;
|
|
}
|
|
|
|
laserGroup = new THREE.Group();
|
|
laserGroup.name = 'live-laser-scan';
|
|
|
|
const fillGeometry = new THREE.BufferGeometry();
|
|
laserFillMesh = new THREE.Mesh(
|
|
fillGeometry,
|
|
new THREE.MeshBasicMaterial({
|
|
color: 0x005cff,
|
|
transparent: true,
|
|
opacity: 0.6,
|
|
side: THREE.DoubleSide,
|
|
depthWrite: false,
|
|
}),
|
|
);
|
|
laserFillMesh.frustumCulled = false;
|
|
|
|
const lineGeometry = new THREE.BufferGeometry();
|
|
laserLineSegments = new THREE.LineSegments(
|
|
lineGeometry,
|
|
new THREE.LineBasicMaterial({ color: 0x3366ff, transparent: true, opacity: 0.5 }),
|
|
);
|
|
laserLineSegments.frustumCulled = false;
|
|
|
|
const openSpaceGeometry = new THREE.BufferGeometry();
|
|
laserOpenSpaceLineSegments = new THREE.LineSegments(
|
|
openSpaceGeometry,
|
|
new THREE.LineBasicMaterial({ color: 0x6f9dff, transparent: true, opacity: 0.28 }),
|
|
);
|
|
laserOpenSpaceLineSegments.frustumCulled = false;
|
|
|
|
laserFrontBeam = new THREE.Mesh(
|
|
new THREE.CylinderGeometry(0.006, 0.006, 1, 16),
|
|
new THREE.MeshBasicMaterial({ color: 0x005cff }),
|
|
);
|
|
laserFrontBeam.frustumCulled = false;
|
|
|
|
laserGroup.add(laserFillMesh, laserLineSegments, laserOpenSpaceLineSegments, laserFrontBeam);
|
|
sdfRoot.add(laserGroup);
|
|
}
|
|
|
|
function updateLaserGeometry(geometry, values) {
|
|
let position = geometry.getAttribute('position');
|
|
if (!position || position.array.length < values.length) {
|
|
const capacity = Math.max(1024, Math.ceil(values.length * 1.25));
|
|
position = new THREE.BufferAttribute(new Float32Array(capacity), 3);
|
|
position.setUsage(THREE.DynamicDrawUsage);
|
|
geometry.setAttribute('position', position);
|
|
}
|
|
position.array.set(values);
|
|
position.needsUpdate = true;
|
|
geometry.setDrawRange(0, values.length / 3);
|
|
}
|
|
|
|
function clearLaserScan() {
|
|
if (!laserGroup) return;
|
|
laserGroup.visible = false;
|
|
laserFillMesh.visible = false;
|
|
laserLineSegments.visible = false;
|
|
laserOpenSpaceLineSegments.visible = false;
|
|
laserFrontBeam.visible = false;
|
|
}
|
|
|
|
function setNavigationSensorVisible(visible) {
|
|
navigationSensorVisible = visible;
|
|
sensorToggleButton.classList.toggle('active', visible);
|
|
sensorToggleButton.setAttribute('aria-pressed', String(visible));
|
|
if (sensorPointCloudLayer) sensorPointCloudLayer.visible = visible;
|
|
if (!visible) {
|
|
if (sensorPointCloudRefreshTimer) {
|
|
window.clearInterval(sensorPointCloudRefreshTimer);
|
|
sensorPointCloudRefreshTimer = null;
|
|
}
|
|
lastLaserRenderAtMs = 0;
|
|
clearLaserScan();
|
|
clearSensorPointCloud();
|
|
drawMiniMap(latestSlamState);
|
|
return;
|
|
}
|
|
if (SHOW_SENSOR_POINT_CLOUD) {
|
|
void refreshSensorPointCloud();
|
|
if (!sensorPointCloudRefreshTimer) {
|
|
sensorPointCloudRefreshTimer = window.setInterval(() => void refreshSensorPointCloud(), 750);
|
|
}
|
|
}
|
|
if (latestScanMetadata && latestScanRanges) {
|
|
drawLaserScan({ ...latestScanMetadata, ranges: latestScanRanges });
|
|
}
|
|
drawMiniMap(latestSlamState);
|
|
}
|
|
|
|
function drawLaserScan(scan, scanPose = (
|
|
NAV2_VIEW_MODE === 'navigation'
|
|
? (navigationSceneUsesMapFrame() ? latestSlamState?.scan_pose_map : latestSlamState?.scan_pose)
|
|
: latestSlamState?.scan_pose_map
|
|
) || null) {
|
|
// Live laser data is controlled exclusively by the sensor toggle. Do not
|
|
// let a state or binary websocket update redraw it while the toggle is off.
|
|
if (!navigationSensorVisible || (NAV2_VIEW_MODE === 'navigation' && !(navigationInitialPoseSet || navigationLocalizationReady))) {
|
|
clearLaserScan();
|
|
return;
|
|
}
|
|
if (!scan?.ranges?.length) return;
|
|
// The state socket and scan binary socket can deliver the same scan nearly
|
|
// together. Rebuilding transparent scan geometry twice stalls the viewer.
|
|
const now = performance.now();
|
|
if (now - lastLaserRenderAtMs < LASER_RENDER_INTERVAL_MS) return;
|
|
lastLaserRenderAtMs = now;
|
|
clearLaserScan();
|
|
|
|
const sourcePose = scanPose || currentRobotPose;
|
|
const renderPose = USE_MKMINI_MODEL && scanPose
|
|
? {
|
|
...sourcePose,
|
|
x: sourcePose.x - Math.cos(sourcePose.yaw || 0) * MKMINI_LASER_RENDER_BACKWARD_OFFSET,
|
|
y: sourcePose.y - Math.sin(sourcePose.yaw || 0) * MKMINI_LASER_RENDER_BACKWARD_OFFSET,
|
|
}
|
|
: sourcePose;
|
|
const origin = scanPose
|
|
? new THREE.Vector3(renderPose.x, renderPose.y, renderPose.z || 0)
|
|
: laserOriginFromPose(renderPose);
|
|
const rangeMax = Number(scan.range_max || 3.5);
|
|
const rangeMin = Number(scan.range_min || 0.12);
|
|
const angleMin = Number(scan.angle_min || 0);
|
|
const angleInc = Number(scan.angle_increment || (Math.PI * 2 / scan.ranges.length));
|
|
|
|
const endpoints = [];
|
|
const linePoints = [];
|
|
const openSpaceLinePoints = [];
|
|
const showOpenSpaceRays = NAV2_VIEW_MODE === 'slam';
|
|
for (let i = 0; i < scan.ranges.length; i++) {
|
|
const raw = scan.ranges[i];
|
|
const valid = Number.isFinite(raw) && raw >= rangeMin && raw < rangeMax;
|
|
const d = valid ? Math.min(raw, rangeMax) : rangeMax;
|
|
const a = angleMin + i * angleInc;
|
|
const dir = laserDirectionFromPose(renderPose, a);
|
|
const startD = Math.min(LASER_HOLE_R, d);
|
|
const start = origin.clone().addScaledVector(dir, startD);
|
|
const point = origin.clone().addScaledVector(dir, d);
|
|
endpoints.push({ start, point, hit: valid });
|
|
if (valid) {
|
|
linePoints.push(start.x, start.y, start.z, point.x, point.y, point.z);
|
|
} else if (showOpenSpaceRays && i % LASER_OPEN_SPACE_RAY_STRIDE === 0) {
|
|
// Match Gazebo's ray visualization: no-return rays remain visible, but
|
|
// deliberately subdued so detected obstacles stay the primary signal.
|
|
openSpaceLinePoints.push(start.x, start.y, start.z, point.x, point.y, point.z);
|
|
}
|
|
}
|
|
|
|
const fillPoints = [];
|
|
for (let i = 0; i < endpoints.length; i++) {
|
|
const curr = endpoints[i];
|
|
const next = endpoints[(i + 1) % endpoints.length];
|
|
if (!curr.hit || !next.hit) continue;
|
|
fillPoints.push(curr.start.x, curr.start.y, curr.start.z);
|
|
fillPoints.push(curr.point.x, curr.point.y, curr.point.z);
|
|
fillPoints.push(next.point.x, next.point.y, next.point.z);
|
|
fillPoints.push(curr.start.x, curr.start.y, curr.start.z);
|
|
fillPoints.push(next.point.x, next.point.y, next.point.z);
|
|
fillPoints.push(next.start.x, next.start.y, next.start.z);
|
|
}
|
|
|
|
ensureLaserScanLayer();
|
|
updateLaserGeometry(laserFillMesh.geometry, fillPoints);
|
|
laserFillMesh.visible = fillPoints.length > 0;
|
|
updateLaserGeometry(laserLineSegments.geometry, linePoints);
|
|
laserLineSegments.visible = linePoints.length > 0;
|
|
updateLaserGeometry(laserOpenSpaceLineSegments.geometry, openSpaceLinePoints);
|
|
laserOpenSpaceLineSegments.visible = openSpaceLinePoints.length > 0;
|
|
|
|
const frontIndex = Math.max(0, Math.min(scan.ranges.length - 1, Math.round((0 - angleMin) / angleInc)));
|
|
const front = endpoints[frontIndex];
|
|
// Keep the forward direction beam even in open space. It is the visual
|
|
// heading reference used in the TurtleBot SLAM view, not only a hit marker.
|
|
if (front) {
|
|
const delta = new THREE.Vector3(
|
|
front.point.x - front.start.x,
|
|
front.point.y - front.start.y,
|
|
front.point.z - front.start.z
|
|
);
|
|
const len = delta.length();
|
|
if (len > 0.01) {
|
|
laserFrontBeam.position.set(
|
|
(front.start.x + front.point.x) / 2,
|
|
(front.start.y + front.point.y) / 2,
|
|
(front.start.z + front.point.z) / 2
|
|
);
|
|
laserFrontBeam.scale.set(1, len, 1);
|
|
laserFrontBeam.quaternion.setFromUnitVectors(new THREE.Vector3(0, 1, 0), delta.normalize());
|
|
laserFrontBeam.visible = true;
|
|
} else {
|
|
laserFrontBeam.visible = false;
|
|
}
|
|
} else {
|
|
laserFrontBeam.visible = false;
|
|
}
|
|
}
|
|
|
|
function updatePointGeometry(geometry, points) {
|
|
if (!geometry || !Array.isArray(points)) return;
|
|
const positions = new Float32Array(points.length * 3);
|
|
let count = 0;
|
|
|
|
for (const point of points) {
|
|
if (!Array.isArray(point) || point.length < 3) continue;
|
|
const x = Number(point[0]);
|
|
const y = Number(point[1]);
|
|
const z = Number(point[2]);
|
|
if (!Number.isFinite(x) || !Number.isFinite(y) || !Number.isFinite(z)) continue;
|
|
positions[count++] = x;
|
|
positions[count++] = y;
|
|
positions[count++] = z;
|
|
}
|
|
|
|
if (count === 0) {
|
|
geometry.deleteAttribute('position');
|
|
geometry.boundingBox = null;
|
|
geometry.boundingSphere = null;
|
|
return;
|
|
}
|
|
|
|
geometry.setAttribute('position', new THREE.BufferAttribute(positions.slice(0, count), 3));
|
|
geometry.computeBoundingSphere();
|
|
}
|
|
|
|
function updatePointCloud(payload) {
|
|
if (!pointCloudLayer) return;
|
|
const frameId = String(payload?.frame_id || '').replace(/^\/+/, '');
|
|
// RTAB-Map's /cloud_map is a complete, transient map in `map`. The raw
|
|
// LiDAR fallback is useful before that arrives, but must never overwrite a
|
|
// loaded navigation map after localization starts.
|
|
if (frameId !== 'map' && hasGlobalMapPointCloud) return;
|
|
updatePointGeometry(pointCloudGeometry, payload?.points);
|
|
if (frameId === 'map') {
|
|
hasGlobalMapPointCloud = true;
|
|
// The saved 3D cloud is now available. The occupancy grid was only a
|
|
// startup fallback while the exported cloud was loading.
|
|
if (navigationMapMesh && SHOW_POINT_CLOUD) {
|
|
navigationMapRoot.remove(navigationMapMesh);
|
|
navigationMapMesh.geometry.dispose();
|
|
navigationMapMesh.material.dispose();
|
|
navigationMapMesh = null;
|
|
navigationMapTexture?.dispose();
|
|
navigationMapTexture = null;
|
|
}
|
|
}
|
|
const transform = payload?.transform;
|
|
if (!transform || !Number.isFinite(Number(transform.x)) || !Number.isFinite(Number(transform.y))) {
|
|
// A cloud already authored in `map` does not need a transform. Hide
|
|
// clouds in an unresolved local frame instead of drawing them under or
|
|
// away from the saved navigation map.
|
|
pointCloudLayer.visible = frameId === 'map';
|
|
pointCloudLayer.position.set(0, 0, 0);
|
|
pointCloudLayer.rotation.set(0, 0, 0);
|
|
return;
|
|
}
|
|
pointCloudLayer.position.set(Number(transform.x), Number(transform.y), Number(transform.z) || 0);
|
|
pointCloudLayer.rotation.set(0, 0, Number(transform.yaw) || 0);
|
|
pointCloudLayer.visible = true;
|
|
}
|
|
|
|
function updateSensorPointCloud(payload) {
|
|
if (!sensorPointCloudLayer) return;
|
|
const receivedAt = Number(payload?.received_at);
|
|
if (!Number.isFinite(receivedAt) || Date.now() - receivedAt * 1000 > SENSOR_POINT_CLOUD_MAX_AGE_MS) {
|
|
clearSensorPointCloud();
|
|
return;
|
|
}
|
|
updatePointGeometry(sensorPointCloudGeometry, payload?.points);
|
|
const transform = payload?.transform;
|
|
if (!transform || !Number.isFinite(Number(transform.x)) || !Number.isFinite(Number(transform.y))) {
|
|
sensorPointCloudLayer.visible = false;
|
|
return;
|
|
}
|
|
sensorPointCloudLayer.position.set(Number(transform.x), Number(transform.y), Number(transform.z) || 0);
|
|
sensorPointCloudLayer.rotation.set(0, 0, Number(transform.yaw) || 0);
|
|
sensorPointCloudLayer.visible = navigationSensorVisible;
|
|
}
|
|
|
|
function clearSensorPointCloud() {
|
|
if (!sensorPointCloudLayer || !sensorPointCloudGeometry) return;
|
|
updatePointGeometry(sensorPointCloudGeometry, []);
|
|
sensorPointCloudLayer.visible = false;
|
|
}
|
|
|
|
async function refreshPointCloud() {
|
|
if (!SHOW_POINT_CLOUD || savedMapPointCloudLoaded || pointCloudRequestInFlight) return;
|
|
pointCloudRequestInFlight = true;
|
|
try {
|
|
const response = await fetch(`${BACKEND_URL}/point-cloud`);
|
|
if (!response.ok) return;
|
|
const payload = await response.json();
|
|
updatePointCloud(payload);
|
|
// A navigation map exported from the selected DB is immutable during this
|
|
// session. Keep polling only for live 3D SLAM clouds.
|
|
if (payload?.saved_map === true) {
|
|
savedMapPointCloudLoaded = true;
|
|
if (pointCloudRefreshTimer) {
|
|
window.clearInterval(pointCloudRefreshTimer);
|
|
pointCloudRefreshTimer = null;
|
|
}
|
|
}
|
|
} catch (_) {
|
|
// The 2D SLAM view remains usable while the 3D point-cloud topic starts.
|
|
} finally {
|
|
pointCloudRequestInFlight = false;
|
|
}
|
|
}
|
|
|
|
async function refreshSensorPointCloud() {
|
|
if (!SHOW_SENSOR_POINT_CLOUD || !navigationSensorVisible || sensorPointCloudRequestInFlight) return;
|
|
sensorPointCloudRequestInFlight = true;
|
|
try {
|
|
const response = await fetch(`${BACKEND_URL}/sensor-point-cloud`);
|
|
if (!response.ok) {
|
|
clearSensorPointCloud();
|
|
return;
|
|
}
|
|
updateSensorPointCloud(await response.json());
|
|
} catch (_) {
|
|
// The static 3D map remains usable while the live lidar stream starts.
|
|
} finally {
|
|
sensorPointCloudRequestInFlight = false;
|
|
}
|
|
}
|
|
|
|
function setCameraPreviewAvailable(available) {
|
|
cameraPreviewWrap?.classList.toggle('is-visible', available);
|
|
if (available) return;
|
|
if (cameraFrameObjectUrl) {
|
|
URL.revokeObjectURL(cameraFrameObjectUrl);
|
|
cameraFrameObjectUrl = null;
|
|
}
|
|
if (cameraPreview) cameraPreview.removeAttribute('src');
|
|
}
|
|
|
|
async function refreshCameraFrame() {
|
|
if (!ENABLE_CAMERA_PREVIEW || !REAL_RUNTIME || cameraFrameRequestInFlight) return;
|
|
cameraFrameRequestInFlight = true;
|
|
try {
|
|
const response = await fetch(`${BACKEND_URL}/camera-frame`, { cache: 'no-store' });
|
|
if (!response.ok) {
|
|
setCameraPreviewAvailable(false);
|
|
return;
|
|
}
|
|
const nextUrl = URL.createObjectURL(await response.blob());
|
|
const previousUrl = cameraFrameObjectUrl;
|
|
cameraFrameObjectUrl = nextUrl;
|
|
if (cameraPreview) cameraPreview.src = nextUrl;
|
|
if (previousUrl) URL.revokeObjectURL(previousUrl);
|
|
setCameraPreviewAvailable(true);
|
|
} catch (_) {
|
|
setCameraPreviewAvailable(false);
|
|
} finally {
|
|
cameraFrameRequestInFlight = false;
|
|
}
|
|
}
|
|
|
|
function drawMiniMap(state) {
|
|
pendingMiniMapState = state;
|
|
if (miniMapRenderTimer) return;
|
|
|
|
const elapsed = performance.now() - lastMiniMapRenderAtMs;
|
|
const wait = Math.max(0, MINI_MAP_RENDER_INTERVAL_MS - elapsed);
|
|
miniMapRenderTimer = window.setTimeout(() => {
|
|
miniMapRenderTimer = null;
|
|
lastMiniMapRenderAtMs = performance.now();
|
|
drawMiniMapNow(pendingMiniMapState);
|
|
}, wait);
|
|
}
|
|
|
|
function drawMiniMapNow(state) {
|
|
if (!miniCtx) return;
|
|
const w = miniMap.width;
|
|
const h = miniMap.height;
|
|
miniCtx.clearRect(0, 0, w, h);
|
|
miniCtx.fillStyle = '#111520';
|
|
miniCtx.fillRect(0, 0, w, h);
|
|
|
|
if (latestMiniMapGrid?.data?.length && latestMiniMapGrid.width && latestMiniMapGrid.height && !miniMapBitmap) {
|
|
const grid = latestMiniMapGrid;
|
|
const img = miniCtx.createImageData(grid.width, grid.height);
|
|
for (let y = 0; y < grid.height; y++) {
|
|
for (let x = 0; x < grid.width; x++) {
|
|
const src = (grid.height - 1 - y) * grid.width + x;
|
|
const dst = (y * grid.width + x) * 4;
|
|
const v = grid.data[src];
|
|
let c = 160;
|
|
if (v < 0) c = 112;
|
|
else if (v === 0) c = 214;
|
|
else c = Math.max(32, 210 - Math.round(v * 1.8));
|
|
img.data[dst] = c;
|
|
img.data[dst + 1] = c;
|
|
img.data[dst + 2] = c;
|
|
img.data[dst + 3] = 255;
|
|
}
|
|
}
|
|
|
|
miniMapBitmap = document.createElement('canvas');
|
|
miniMapBitmap.width = grid.width;
|
|
miniMapBitmap.height = grid.height;
|
|
miniMapBitmap.getContext('2d').putImageData(img, 0, 0);
|
|
}
|
|
|
|
const grid = latestMiniMapGrid;
|
|
if (grid && miniMapBitmap) {
|
|
const scale = Math.min(w / grid.width, h / grid.height);
|
|
const dw = grid.width * scale;
|
|
const dh = grid.height * scale;
|
|
const dx = (w - dw) / 2;
|
|
const dy = (h - dh) / 2;
|
|
miniCtx.imageSmoothingEnabled = false;
|
|
miniCtx.drawImage(miniMapBitmap, dx, dy, dw, dh);
|
|
|
|
const worldToMini = (wx, wy) => {
|
|
const yaw = Number(grid.origin.yaw || 0);
|
|
const c = Math.cos(yaw);
|
|
const s = Math.sin(yaw);
|
|
const x = wx - grid.origin.x;
|
|
const y = wy - grid.origin.y;
|
|
const localX = c * x + s * y;
|
|
const localY = -s * x + c * y;
|
|
return {
|
|
x: dx + (localX / grid.resolution) * scale,
|
|
y: dy + (grid.height - localY / grid.resolution) * scale,
|
|
};
|
|
};
|
|
|
|
const mapFrame = { x:0, y:0, yaw:0 };
|
|
const odomFrame = state?.tf?.map_to_odom || { x:0, y:0, yaw:0 };
|
|
const robotFrame = state?.pose_map || currentRobotPose;
|
|
const scanFrame = state?.scan_pose_map || null;
|
|
const frameLen = Math.max(grid.resolution * 12, 0.18);
|
|
|
|
if ((NAV2_VIEW_MODE !== 'navigation' || navigationSensorVisible) && navigationInitialPoseSet && state?.scan?.ranges?.length && (state?.pose_map || state?.pose)) {
|
|
const sourceFrame = scanFrame || robotFrame;
|
|
const c = Math.cos(sourceFrame.yaw);
|
|
const s = Math.sin(sourceFrame.yaw);
|
|
const laserOrigin = scanFrame
|
|
? { x: sourceFrame.x, y: sourceFrame.y }
|
|
: {
|
|
x: sourceFrame.x + c * LASER_OFFSET_X - s * LASER_OFFSET_Y,
|
|
y: sourceFrame.y + s * LASER_OFFSET_X + c * LASER_OFFSET_Y,
|
|
};
|
|
const angleMin = Number(state.scan.angle_min || 0);
|
|
const angleInc = Number(state.scan.angle_increment || (Math.PI * 2 / state.scan.ranges.length));
|
|
const rangeMin = Number(state.scan.range_min || 0.12);
|
|
const rangeMax = Number(state.scan.range_max || 3.5);
|
|
const blink = 0.45 + 0.45 * Math.abs(Math.sin(Date.now() / 180));
|
|
|
|
miniCtx.fillStyle = `rgba(255, 0, 0, ${blink.toFixed(3)})`;
|
|
for (let i = 0; i < state.scan.ranges.length; i += 2) {
|
|
const d = state.scan.ranges[i];
|
|
if (!Number.isFinite(d) || d < rangeMin || d >= rangeMax) continue;
|
|
const a = sourceFrame.yaw + angleMin + i * angleInc;
|
|
const p = worldToMini(
|
|
laserOrigin.x + Math.cos(a) * d,
|
|
laserOrigin.y + Math.sin(a) * d
|
|
);
|
|
miniCtx.beginPath();
|
|
miniCtx.arc(p.x, p.y, 1.2, 0, Math.PI * 2);
|
|
miniCtx.fill();
|
|
}
|
|
}
|
|
|
|
function drawLink(a, b) {
|
|
const p0 = worldToMini(a.x, a.y);
|
|
const p1 = worldToMini(b.x, b.y);
|
|
miniCtx.strokeStyle = 'rgba(255, 230, 0, .9)';
|
|
miniCtx.lineWidth = 1;
|
|
miniCtx.beginPath();
|
|
miniCtx.moveTo(p0.x, p0.y);
|
|
miniCtx.lineTo(p1.x, p1.y);
|
|
miniCtx.stroke();
|
|
}
|
|
|
|
function drawFrame(frame, label) {
|
|
const o = worldToMini(frame.x, frame.y);
|
|
const xEnd = worldToMini(frame.x + Math.cos(frame.yaw) * frameLen, frame.y + Math.sin(frame.yaw) * frameLen);
|
|
const yEnd = worldToMini(frame.x - Math.sin(frame.yaw) * frameLen, frame.y + Math.cos(frame.yaw) * frameLen);
|
|
|
|
miniCtx.lineWidth = 2;
|
|
miniCtx.strokeStyle = '#d71920';
|
|
miniCtx.beginPath();
|
|
miniCtx.moveTo(o.x, o.y);
|
|
miniCtx.lineTo(xEnd.x, xEnd.y);
|
|
miniCtx.stroke();
|
|
|
|
miniCtx.strokeStyle = '#22aa33';
|
|
miniCtx.beginPath();
|
|
miniCtx.moveTo(o.x, o.y);
|
|
miniCtx.lineTo(yEnd.x, yEnd.y);
|
|
miniCtx.stroke();
|
|
|
|
miniCtx.fillStyle = '#005cff';
|
|
miniCtx.beginPath();
|
|
miniCtx.arc(o.x, o.y, 3, 0, Math.PI * 2);
|
|
miniCtx.fill();
|
|
|
|
miniCtx.fillStyle = 'rgba(255,255,255,.92)';
|
|
miniCtx.strokeStyle = 'rgba(0,0,0,.75)';
|
|
miniCtx.lineWidth = 3;
|
|
miniCtx.font = '11px sans-serif';
|
|
miniCtx.strokeText(label, o.x + 5, o.y - 5);
|
|
miniCtx.fillText(label, o.x + 5, o.y - 5);
|
|
}
|
|
|
|
drawLink(mapFrame, odomFrame);
|
|
if (shouldRenderRobot() && (state?.pose || state?.pose_map)) drawLink(odomFrame, robotFrame);
|
|
drawFrame(mapFrame, 'map');
|
|
drawFrame(odomFrame, 'odom');
|
|
if (shouldRenderRobot() && (state?.pose || state?.pose_map)) drawFrame(robotFrame, 'robot');
|
|
return;
|
|
}
|
|
|
|
miniCtx.fillStyle = 'rgba(255,255,255,.68)';
|
|
miniCtx.font = '600 13px sans-serif';
|
|
miniCtx.textAlign = 'center';
|
|
miniCtx.textBaseline = 'middle';
|
|
miniCtx.fillText(
|
|
SHOW_POINT_CLOUD && state?.point_cloud
|
|
? '3D 포인트클라우드를 표시 중입니다.'
|
|
: '모듈을 연결해주세요.',
|
|
w / 2,
|
|
h / 2,
|
|
);
|
|
miniCtx.textAlign = 'start';
|
|
miniCtx.textBaseline = 'alphabetic';
|
|
}
|
|
|
|
function updateMiniMapMetadata(map) {
|
|
if (!map?.width || !map?.height) return;
|
|
// MAP1 and its metadata are sent as separate WebSocket frames. Accept the
|
|
// first metadata frame even when its binary payload arrived first; only a
|
|
// fully rendered saved navigation map is locked against later local maps.
|
|
if (navigationMapLocked && navigationMapMesh) return;
|
|
latestMiniMapGrid = {
|
|
...(latestMiniMapGrid || {}),
|
|
...map,
|
|
data: latestMiniMapGrid?.data || null,
|
|
};
|
|
// Map metadata and its binary occupancy payload arrive on separate socket
|
|
// frames. When the payload wins the race, retry now that origin/resolution
|
|
// are available so the floor map is not left empty.
|
|
if (latestMiniMapGrid.data?.length) renderNavigationOccupancyMap();
|
|
}
|
|
|
|
function updateNavigationMapTransform() {
|
|
if (NAV2_VIEW_MODE !== 'navigation') return;
|
|
const transform = navigationMapToOdom || { x:0, y:0, yaw:0 };
|
|
if (navigationSceneUsesMapFrame()) {
|
|
navigationMapRoot.position.set(0, 0, 0);
|
|
navigationMapRoot.rotation.set(0, 0, 0);
|
|
// The saved map and the Gazebo world share the coordinates captured
|
|
// during SLAM. AMCL's live map -> odom correction applies to the robot
|
|
// estimate and must not move the static world geometry.
|
|
gazeboWorldRoot.position.set(0, 0, 0);
|
|
gazeboWorldRoot.rotation.set(0, 0, 0);
|
|
return;
|
|
}
|
|
gazeboWorldRoot.position.set(0, 0, 0);
|
|
gazeboWorldRoot.rotation.set(0, 0, 0);
|
|
const yaw = transform.yaw || 0;
|
|
const c = Math.cos(yaw);
|
|
const s = Math.sin(yaw);
|
|
// `map_to_odom` is map <- odom. The map mesh is authored in map
|
|
// coordinates, so the odom scene needs its inverse transform.
|
|
navigationMapRoot.position.set(
|
|
-c * (transform.x || 0) - s * (transform.y || 0),
|
|
s * (transform.x || 0) - c * (transform.y || 0),
|
|
0,
|
|
);
|
|
navigationMapRoot.rotation.set(0, 0, -yaw);
|
|
}
|
|
|
|
function renderNavigationOccupancyMap() {
|
|
const grid = latestMiniMapGrid;
|
|
if (NAV2_VIEW_MODE !== 'navigation' || !grid?.data?.length || !grid.width || !grid.height || !grid.resolution || !grid.origin) return false;
|
|
// The occupancy grid is a short-lived fallback for a saved 3D map. It
|
|
// keeps the scene usable if the point-cloud export is still starting.
|
|
if (SHOW_POINT_CLOUD && hasGlobalMapPointCloud) return false;
|
|
|
|
const canvas = document.createElement('canvas');
|
|
canvas.width = grid.width;
|
|
canvas.height = grid.height;
|
|
const context = canvas.getContext('2d');
|
|
const image = context.createImageData(grid.width, grid.height);
|
|
for (let y = 0; y < grid.height; y++) {
|
|
for (let x = 0; x < grid.width; x++) {
|
|
const source = (grid.height - 1 - y) * grid.width + x;
|
|
const target = (y * grid.width + x) * 4;
|
|
const value = grid.data[source];
|
|
const color = value < 0 ? 102 : value >= 50 ? 24 : 226;
|
|
image.data[target] = color;
|
|
image.data[target + 1] = color;
|
|
image.data[target + 2] = color;
|
|
image.data[target + 3] = value < 0 ? 170 : 235;
|
|
}
|
|
}
|
|
context.putImageData(image, 0, 0);
|
|
|
|
if (navigationMapMesh) {
|
|
navigationMapRoot.remove(navigationMapMesh);
|
|
navigationMapMesh.geometry.dispose();
|
|
navigationMapMesh.material.dispose();
|
|
}
|
|
navigationMapTexture?.dispose();
|
|
navigationMapTexture = new THREE.CanvasTexture(canvas);
|
|
navigationMapTexture.colorSpace = THREE.SRGBColorSpace;
|
|
navigationMapTexture.magFilter = THREE.NearestFilter;
|
|
navigationMapTexture.minFilter = THREE.NearestFilter;
|
|
navigationMapTexture.flipY = true;
|
|
|
|
const width = grid.width * grid.resolution;
|
|
const height = grid.height * grid.resolution;
|
|
navigationMapMesh = new THREE.Mesh(
|
|
new THREE.PlaneGeometry(width, height),
|
|
new THREE.MeshBasicMaterial({ map:navigationMapTexture, transparent:true, opacity:0.58, side:THREE.DoubleSide, depthWrite:false }),
|
|
);
|
|
const originYaw = Number(grid.origin.yaw || 0);
|
|
const centerX = grid.origin.x + Math.cos(originYaw) * width / 2 - Math.sin(originYaw) * height / 2;
|
|
const centerY = grid.origin.y + Math.sin(originYaw) * width / 2 + Math.cos(originYaw) * height / 2;
|
|
navigationMapMesh.position.set(centerX, centerY, 0.012);
|
|
navigationMapMesh.rotation.z = originYaw;
|
|
navigationMapRoot.add(navigationMapMesh);
|
|
updateNavigationMapTransform();
|
|
|
|
// A saved 3D map is the navigation reference. RTAB-Map can publish a small
|
|
// local occupancy update later on /map; replacing the initial complete map
|
|
// with it makes the floor map appear to shrink during localization.
|
|
if (LOCK_NAVIGATION_MAP) navigationMapLocked = true;
|
|
|
|
if (!navigationMapFocused) {
|
|
const sceneCenter = navigationSceneUsesMapFrame()
|
|
? { x:centerX, y:centerY }
|
|
: mapToOdom({ x:centerX, y:centerY, yaw:originYaw });
|
|
const center = sdfPointToWorld(sceneCenter.x, sceneCenter.y, 0);
|
|
const distance = Math.max(width, height) * 1.1;
|
|
controls.target.copy(center);
|
|
camera.position.set(center.x + distance * .62, center.y + distance * .86, center.z + distance * .62);
|
|
camera.lookAt(center);
|
|
navigationMapFocused = true;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
function applyMapBinary(buffer) {
|
|
if (navigationMapLocked) return;
|
|
const view = new DataView(buffer);
|
|
if (
|
|
buffer.byteLength < 12 ||
|
|
view.getUint8(0) !== 0x4d ||
|
|
view.getUint8(1) !== 0x41 ||
|
|
view.getUint8(2) !== 0x50 ||
|
|
view.getUint8(3) !== 0x31
|
|
) {
|
|
console.warn('unknown binary websocket frame');
|
|
return;
|
|
}
|
|
|
|
const width = view.getUint32(4, true);
|
|
const height = view.getUint32(8, true);
|
|
const data = new Int8Array(buffer, 12);
|
|
if (data.length !== width * height) {
|
|
console.warn(`invalid map binary size: ${data.length} != ${width * height}`);
|
|
return;
|
|
}
|
|
|
|
latestMiniMapGrid = {
|
|
...(latestMiniMapGrid || {}),
|
|
width,
|
|
height,
|
|
data,
|
|
};
|
|
miniMapBitmap = null;
|
|
// Do not lock a selected navigation map before its separate metadata frame
|
|
// supplies resolution and origin. Otherwise Three.js receives valid /map
|
|
// data but cannot construct a plane from it.
|
|
renderNavigationOccupancyMap();
|
|
drawMiniMap(latestSlamState);
|
|
downloadMapButton.disabled = false;
|
|
}
|
|
|
|
function applyScanBinary(buffer) {
|
|
const view = new DataView(buffer);
|
|
if (
|
|
buffer.byteLength < 8 ||
|
|
view.getUint8(0) !== 0x53 ||
|
|
view.getUint8(1) !== 0x43 ||
|
|
view.getUint8(2) !== 0x4e ||
|
|
view.getUint8(3) !== 0x31
|
|
) {
|
|
console.warn('unknown binary websocket frame');
|
|
return;
|
|
}
|
|
|
|
const count = view.getUint32(4, true);
|
|
if (buffer.byteLength !== 8 + count * 4) {
|
|
console.warn(`invalid scan binary size: ${buffer.byteLength} != ${8 + count * 4}`);
|
|
return;
|
|
}
|
|
|
|
latestScanRanges = new Float32Array(buffer, 8, count);
|
|
if (!latestScanMetadata) return;
|
|
const scan = { ...latestScanMetadata, ranges: latestScanRanges };
|
|
drawLaserScan(scan);
|
|
drawMiniMap({ ...(latestSlamState || {}), scan });
|
|
}
|
|
|
|
function applyBinaryFrame(buffer) {
|
|
if (buffer.byteLength < 4) return;
|
|
const view = new DataView(buffer, 0, 4);
|
|
const magic = String.fromCharCode(
|
|
view.getUint8(0),
|
|
view.getUint8(1),
|
|
view.getUint8(2),
|
|
view.getUint8(3)
|
|
);
|
|
if (magic === 'MAP1') applyMapBinary(buffer);
|
|
else if (magic === 'SCN1') applyScanBinary(buffer);
|
|
else console.warn(`unknown binary websocket frame: ${magic}`);
|
|
}
|
|
|
|
async function applySocketBinaryPayload(payload) {
|
|
if (payload instanceof ArrayBuffer) {
|
|
applyBinaryFrame(payload);
|
|
return true;
|
|
}
|
|
if (payload instanceof Blob) {
|
|
applyBinaryFrame(await payload.arrayBuffer());
|
|
return true;
|
|
}
|
|
if (ArrayBuffer.isView(payload)) {
|
|
applyBinaryFrame(payload.buffer.slice(payload.byteOffset, payload.byteOffset + payload.byteLength));
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
function mapToPcd(grid) {
|
|
const points = [];
|
|
const resolution = Number(grid.resolution || 0);
|
|
const originX = Number(grid.origin?.x || 0);
|
|
const originY = Number(grid.origin?.y || 0);
|
|
const originYaw = Number(grid.origin?.yaw || 0);
|
|
const c = Math.cos(originYaw);
|
|
const s = Math.sin(originYaw);
|
|
|
|
for (let y = 0; y < grid.height; y++) {
|
|
for (let x = 0; x < grid.width; x++) {
|
|
const value = grid.data[y * grid.width + x];
|
|
if (value < 50) continue;
|
|
|
|
const lx = (x + 0.5) * resolution;
|
|
const ly = (y + 0.5) * resolution;
|
|
const wx = originX + c * lx - s * ly;
|
|
const wy = originY + s * lx + c * ly;
|
|
points.push(`${wx.toFixed(4)} ${wy.toFixed(4)} 0.0000`);
|
|
}
|
|
}
|
|
|
|
return [
|
|
'# .PCD v0.7 - Point Cloud Data file format',
|
|
'VERSION 0.7',
|
|
'FIELDS x y z',
|
|
'SIZE 4 4 4',
|
|
'TYPE F F F',
|
|
'COUNT 1 1 1',
|
|
`WIDTH ${points.length}`,
|
|
'HEIGHT 1',
|
|
'VIEWPOINT 0 0 0 1 0 0 0',
|
|
`POINTS ${points.length}`,
|
|
'DATA ascii',
|
|
...points,
|
|
'',
|
|
].join('\n');
|
|
}
|
|
|
|
function mapToPgmBytes(grid) {
|
|
const header = `P5\n# CREATOR: slam-test web viewer\n${grid.width} ${grid.height}\n255\n`;
|
|
const headerBytes = new TextEncoder().encode(header);
|
|
const pixels = new Uint8Array(grid.width * grid.height);
|
|
|
|
for (let y = 0; y < grid.height; y++) {
|
|
for (let x = 0; x < grid.width; x++) {
|
|
const src = (grid.height - 1 - y) * grid.width + x;
|
|
const dst = y * grid.width + x;
|
|
const v = grid.data[src];
|
|
if (v < 0) pixels[dst] = 205;
|
|
else if (v === 0) pixels[dst] = 254;
|
|
else pixels[dst] = Math.max(0, 254 - Math.round(v * 2.54));
|
|
}
|
|
}
|
|
|
|
const out = new Uint8Array(headerBytes.length + pixels.length);
|
|
out.set(headerBytes, 0);
|
|
out.set(pixels, headerBytes.length);
|
|
return out;
|
|
}
|
|
|
|
function mapToYaml(grid, imageName) {
|
|
return [
|
|
`image: ${imageName}`,
|
|
'mode: trinary',
|
|
`resolution: ${Number(grid.resolution || 0)}`,
|
|
`origin: [${Number(grid.origin?.x || 0)}, ${Number(grid.origin?.y || 0)}, ${Number(grid.origin?.yaw || 0)}]`,
|
|
'negate: 0',
|
|
'occupied_thresh: 0.65',
|
|
'free_thresh: 0.196',
|
|
'',
|
|
].join('\n');
|
|
}
|
|
|
|
function downloadBlob(blob, filename) {
|
|
const url = URL.createObjectURL(blob);
|
|
const a = document.createElement('a');
|
|
a.href = url;
|
|
a.download = filename;
|
|
document.body.appendChild(a);
|
|
a.click();
|
|
a.remove();
|
|
URL.revokeObjectURL(url);
|
|
}
|
|
|
|
function downloadMap(format) {
|
|
if (!latestMiniMapGrid?.data?.length) return;
|
|
const stamp = new Date().toISOString().replace(/[:.]/g, '-');
|
|
const baseName = `slam-map-${stamp}`;
|
|
|
|
if (format === 'pcd') {
|
|
downloadBlob(new Blob([mapToPcd(latestMiniMapGrid)], { type:'application/octet-stream' }), `${baseName}.pcd`);
|
|
} else if (format === 'pgm') {
|
|
downloadBlob(new Blob([mapToPgmBytes(latestMiniMapGrid)], { type:'image/x-portable-graymap' }), `${baseName}.pgm`);
|
|
} else if (format === 'yaml') {
|
|
downloadBlob(new Blob([mapToYaml(latestMiniMapGrid, `${baseName}.pgm`)], { type:'text/yaml' }), `${baseName}.yaml`);
|
|
}
|
|
}
|
|
|
|
downloadMapButton.addEventListener('click', async () => {
|
|
if (downloadMapButton.disabled) return;
|
|
downloadMapButton.disabled = true;
|
|
setMapSavePending(true);
|
|
try {
|
|
const response = await fetch(`${BACKEND_URL}/save-map`, {
|
|
method: 'POST',
|
|
headers: { 'content-type': 'application/json' },
|
|
body: JSON.stringify({})
|
|
});
|
|
const payload = await response.json().catch(() => ({}));
|
|
if (!response.ok) throw new Error(payload.detail || payload.message || '맵 저장에 실패했습니다.');
|
|
window.parent.postMessage({ source: 'autonomy-slam-viewer', type: 'map-saved', map: payload.map }, '*');
|
|
} catch (error) {
|
|
window.parent.postMessage({ source: 'autonomy-slam-viewer', type: 'map-save-failed', message: error instanceof Error ? error.message : '맵 저장에 실패했습니다.' }, '*');
|
|
} finally {
|
|
setMapSavePending(false);
|
|
downloadMapButton.disabled = !latestMiniMapGrid?.data?.length;
|
|
focusViewer();
|
|
}
|
|
});
|
|
|
|
pathToggleButton.addEventListener('click', () => {
|
|
setPathVisible(!pathVisible);
|
|
focusViewer();
|
|
});
|
|
|
|
sensorToggleButton.addEventListener('click', () => {
|
|
setNavigationSensorVisible(!navigationSensorVisible);
|
|
focusViewer();
|
|
});
|
|
|
|
cameraPreviewToggle?.addEventListener('click', () => {
|
|
const collapsed = cameraPreviewWrap?.classList.toggle('is-collapsed') ?? false;
|
|
cameraPreviewToggle.setAttribute('aria-expanded', String(!collapsed));
|
|
cameraPreviewToggle.setAttribute('aria-label', collapsed ? '카메라 화면 펼치기' : '카메라 화면 접기');
|
|
});
|
|
|
|
goalToggleButton.addEventListener('click', () => {
|
|
if (goalToggleButton.disabled) return;
|
|
navigationGoalMode = !navigationGoalMode;
|
|
initialPoseMode = false;
|
|
obstaclePlacementMode = false;
|
|
hideObstaclePreview();
|
|
poseToggleButton.classList.remove('active');
|
|
obstacleToggleButton.classList.remove('active');
|
|
obstacleToggleButton.setAttribute('aria-pressed', 'false');
|
|
hideInitialPosePreview();
|
|
goalToggleButton.classList.toggle('active', navigationGoalMode);
|
|
if (!navigationGoalMode) hideNavigationGoalPreview();
|
|
focusViewer();
|
|
});
|
|
|
|
poseToggleButton.addEventListener('click', () => {
|
|
if (poseToggleButton.disabled) return;
|
|
initialPoseMode = !initialPoseMode;
|
|
initialPoseDrag = null;
|
|
navigationGoalMode = false;
|
|
hideNavigationGoalPreview();
|
|
obstaclePlacementMode = false;
|
|
hideObstaclePreview();
|
|
goalToggleButton.classList.remove('active');
|
|
obstacleToggleButton.classList.remove('active');
|
|
obstacleToggleButton.setAttribute('aria-pressed', 'false');
|
|
poseToggleButton.classList.toggle('active', initialPoseMode);
|
|
if (!initialPoseMode) hideInitialPosePreview();
|
|
focusViewer();
|
|
});
|
|
|
|
obstacleToggleButton.addEventListener('click', () => {
|
|
if (obstaclePlacementMode) {
|
|
obstaclePlacementMode = false;
|
|
obstacleToggleButton.classList.remove('active');
|
|
obstacleToggleButton.setAttribute('aria-pressed', 'false');
|
|
clearRuntimeObstacles();
|
|
hideObstaclePreview();
|
|
window.parent.postMessage({ source:'autonomy-slam-viewer', type:'runtime-obstacle-clear' }, '*');
|
|
focusViewer();
|
|
return;
|
|
}
|
|
obstaclePlacementMode = !obstaclePlacementMode;
|
|
navigationGoalMode = false;
|
|
initialPoseMode = false;
|
|
hideNavigationGoalPreview();
|
|
hideInitialPosePreview();
|
|
obstaclePreviewTemplate = null;
|
|
goalToggleButton.classList.remove('active');
|
|
poseToggleButton.classList.remove('active');
|
|
obstacleToggleButton.classList.toggle('active', obstaclePlacementMode);
|
|
obstacleToggleButton.setAttribute('aria-pressed', String(obstaclePlacementMode));
|
|
focusViewer();
|
|
});
|
|
|
|
miniMapToggle.addEventListener('click', () => {
|
|
const hidden = miniMap.classList.toggle('hidden');
|
|
miniMapToggle.textContent = hidden ? '▴' : '▾';
|
|
miniMapToggle.setAttribute('aria-label', hidden ? '미니맵 펼치기' : '미니맵 접기');
|
|
focusViewer();
|
|
});
|
|
|
|
const DEFAULT_CAMERA = {
|
|
position: new THREE.Vector3(4, 6, 5),
|
|
target: new THREE.Vector3(0, 0, 0),
|
|
};
|
|
|
|
function sdfPointToWorld(x, y, z = 0) {
|
|
const point = new THREE.Vector3(x, y, z);
|
|
sdfRoot.updateMatrixWorld();
|
|
return sdfRoot.localToWorld(point);
|
|
}
|
|
|
|
function worldPointToSdf(point) {
|
|
sdfRoot.updateMatrixWorld();
|
|
return sdfRoot.worldToLocal(point.clone());
|
|
}
|
|
|
|
function odomToMap(pose) {
|
|
if (!navigationMapToOdom) return pose;
|
|
const c = Math.cos(navigationMapToOdom.yaw || 0);
|
|
const s = Math.sin(navigationMapToOdom.yaw || 0);
|
|
return {
|
|
x: navigationMapToOdom.x + c * pose.x - s * pose.y,
|
|
y: navigationMapToOdom.y + s * pose.x + c * pose.y,
|
|
yaw: (pose.yaw || 0) + (navigationMapToOdom.yaw || 0),
|
|
};
|
|
}
|
|
|
|
function mapToOdom(pose) {
|
|
if (!navigationMapToOdom) return pose;
|
|
const yaw = navigationMapToOdom.yaw || 0;
|
|
const c = Math.cos(yaw);
|
|
const s = Math.sin(yaw);
|
|
const dx = pose.x - navigationMapToOdom.x;
|
|
const dy = pose.y - navigationMapToOdom.y;
|
|
return {
|
|
x: c * dx + s * dy,
|
|
y: -s * dx + c * dy,
|
|
yaw: (pose.yaw || 0) - yaw,
|
|
};
|
|
}
|
|
|
|
function placeNavigationGoalMarker(x, y, yaw = 0) {
|
|
navigationGoalPose = { x, y, yaw };
|
|
if (!navigationGoalMarker) {
|
|
const marker = new THREE.Group();
|
|
const ring = new THREE.Mesh(
|
|
new THREE.TorusGeometry(0.26, 0.025, 8, 36),
|
|
new THREE.MeshBasicMaterial({ color:0x22c55e })
|
|
);
|
|
marker.add(ring);
|
|
sdfRoot.add(marker);
|
|
navigationGoalMarker = marker;
|
|
}
|
|
updateNavigationGoalMarker();
|
|
}
|
|
|
|
function updateNavigationGoalMarker() {
|
|
if (!navigationGoalMarker || !navigationGoalPose) return;
|
|
const markerPose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame()
|
|
? mapToOdom(navigationGoalPose)
|
|
: navigationGoalPose;
|
|
navigationGoalMarker.position.set(markerPose.x, markerPose.y, 0.05);
|
|
navigationGoalMarker.rotation.z = markerPose.yaw;
|
|
}
|
|
|
|
function setNavigationGoalPreview(pose) {
|
|
if (!navigationGoalPreview) {
|
|
navigationGoalPreview = new THREE.Mesh(
|
|
new THREE.TorusGeometry(0.26, 0.025, 8, 36),
|
|
new THREE.MeshBasicMaterial({ color:0x39ff88, transparent:true, opacity:0.72, depthWrite:false })
|
|
);
|
|
sdfRoot.add(navigationGoalPreview);
|
|
}
|
|
const scenePose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? mapToOdom(pose) : pose;
|
|
navigationGoalPreview.position.set(scenePose.x, scenePose.y, 0.06);
|
|
navigationGoalPreview.rotation.z = scenePose.yaw || 0;
|
|
navigationGoalPreview.visible = true;
|
|
}
|
|
|
|
function hideNavigationGoalPreview() {
|
|
if (navigationGoalPreview) navigationGoalPreview.visible = false;
|
|
}
|
|
|
|
function addRuntimeObstacle(obstacle) {
|
|
const mesh = new THREE.Mesh(
|
|
new THREE.BoxGeometry(obstacle.length, obstacle.depth, obstacle.height),
|
|
new THREE.MeshStandardMaterial({ color:0xd97706, roughness:0.78, metalness:0.03 })
|
|
);
|
|
mesh.position.set(obstacle.x, obstacle.y, obstacle.height / 2);
|
|
mesh.rotation.z = obstacle.yaw;
|
|
mesh.userData.obstacleId = obstacle.id;
|
|
runtimeObstacleObjects.add(mesh);
|
|
}
|
|
|
|
function clearRuntimeObstacles() {
|
|
clearLayoutObjects(runtimeObstacleObjects);
|
|
}
|
|
|
|
function createObstaclePreviewTemplate() {
|
|
return {
|
|
yaw: (Math.floor(Math.random() * 4) * Math.PI) / 2,
|
|
length: 0.7 + Math.random() * 0.9,
|
|
depth: 0.16,
|
|
height: 1.0,
|
|
};
|
|
}
|
|
|
|
function setObstaclePreview(pose) {
|
|
if (!obstaclePreviewTemplate) obstaclePreviewTemplate = createObstaclePreviewTemplate();
|
|
if (!obstaclePreview) {
|
|
obstaclePreview = new THREE.Mesh(
|
|
new THREE.BoxGeometry(obstaclePreviewTemplate.length, obstaclePreviewTemplate.depth, obstaclePreviewTemplate.height),
|
|
new THREE.MeshStandardMaterial({ color:0xf59e0b, transparent:true, opacity:0.45, depthWrite:false })
|
|
);
|
|
sdfRoot.add(obstaclePreview);
|
|
}
|
|
const scenePose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? mapToOdom(pose) : pose;
|
|
obstaclePreview.position.set(scenePose.x, scenePose.y, obstaclePreviewTemplate.height / 2);
|
|
obstaclePreview.rotation.z = obstaclePreviewTemplate.yaw;
|
|
obstaclePreview.visible = true;
|
|
}
|
|
|
|
function hideObstaclePreview() {
|
|
if (obstaclePreview) obstaclePreview.visible = false;
|
|
}
|
|
|
|
function updateNavigationPlan(plan) {
|
|
if (navigationPlanSuppressed) return;
|
|
const poses = Array.isArray(plan?.poses) ? plan.poses : [];
|
|
const signature = poses.map((pose) => `${Number(pose.x).toFixed(3)},${Number(pose.y).toFixed(3)}`).join('|');
|
|
if (signature === navigationPlanSignature) return;
|
|
navigationPlanSignature = signature;
|
|
if (navigationPlanLine) {
|
|
sdfRoot.remove(navigationPlanLine);
|
|
navigationPlanLine.geometry.dispose();
|
|
navigationPlanLine.material.dispose();
|
|
navigationPlanLine = null;
|
|
}
|
|
if (poses.length < 2) return;
|
|
const points = poses.map((pose) => {
|
|
const scenePose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? mapToOdom(pose) : pose;
|
|
return new THREE.Vector3(scenePose.x, scenePose.y, 0.07);
|
|
});
|
|
navigationPlanLine = new THREE.Line(
|
|
new THREE.BufferGeometry().setFromPoints(points),
|
|
new THREE.LineBasicMaterial({ color:0x60a5fa, transparent:true, opacity:0.95 })
|
|
);
|
|
sdfRoot.add(navigationPlanLine);
|
|
}
|
|
|
|
function clearNavigationGoalMarker() {
|
|
if (!navigationGoalMarker) return;
|
|
sdfRoot.remove(navigationGoalMarker);
|
|
disposeObject(navigationGoalMarker);
|
|
navigationGoalMarker = null;
|
|
navigationGoalPose = null;
|
|
}
|
|
|
|
function pickNavigationGoal(event) {
|
|
const rect = renderer.domElement.getBoundingClientRect();
|
|
const mouse = new THREE.Vector2(
|
|
((event.clientX - rect.left) / rect.width) * 2 - 1,
|
|
-((event.clientY - rect.top) / rect.height) * 2 + 1
|
|
);
|
|
navigationRaycaster.setFromCamera(mouse, camera);
|
|
const worldPoint = new THREE.Vector3();
|
|
if (!navigationRaycaster.ray.intersectPlane(navigationGroundPlane, worldPoint)) return null;
|
|
const sdfPoint = worldPointToSdf(worldPoint);
|
|
const mapPoint = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame()
|
|
? odomToMap({ x: sdfPoint.x, y: sdfPoint.y, yaw: currentRobotPose.yaw || 0 })
|
|
: sdfPoint;
|
|
const robotMapPose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame()
|
|
? odomToMap(currentRobotPose)
|
|
: currentRobotPose;
|
|
const headingToGoal = Math.atan2(
|
|
mapPoint.y - (robotMapPose?.y || 0),
|
|
mapPoint.x - (robotMapPose?.x || 0),
|
|
);
|
|
return {
|
|
x: mapPoint.x,
|
|
y: mapPoint.y,
|
|
// A Dubins-based Nav2 planner must know which way the vehicle should face
|
|
// at the destination. The clicked ground point has no yaw, so using zero
|
|
// forced every goal to face east and made otherwise valid paths fail.
|
|
yaw: Number.isFinite(headingToGoal) ? headingToGoal : (mapPoint.yaw || 0),
|
|
};
|
|
}
|
|
|
|
function showInitialPosePreview(pose) {
|
|
if (!posePreviewModel) return;
|
|
const scenePose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? mapToOdom(pose) : pose;
|
|
posePreviewModel.position.set(scenePose.x, scenePose.y, 0.01);
|
|
posePreviewModel.rotation.set(0, 0, scenePose.yaw || 0);
|
|
posePreviewModel.visible = true;
|
|
|
|
if (!initialPoseDrag) {
|
|
if (initialPoseDirectionArrow) initialPoseDirectionArrow.visible = false;
|
|
return;
|
|
}
|
|
if (!initialPoseDirectionArrow) {
|
|
initialPoseDirectionArrow = new THREE.ArrowHelper(
|
|
new THREE.Vector3(1, 0, 0),
|
|
new THREE.Vector3(),
|
|
0.72,
|
|
0xfacc15,
|
|
0.18,
|
|
0.11,
|
|
);
|
|
initialPoseDirectionArrow.name = 'initial-pose-direction';
|
|
initialPoseDirectionArrow.renderOrder = 4;
|
|
sdfRoot.add(initialPoseDirectionArrow);
|
|
}
|
|
initialPoseDirectionArrow.position.set(scenePose.x, scenePose.y, 0.12);
|
|
initialPoseDirectionArrow.setDirection(
|
|
new THREE.Vector3(Math.cos(scenePose.yaw || 0), Math.sin(scenePose.yaw || 0), 0),
|
|
);
|
|
initialPoseDirectionArrow.visible = true;
|
|
}
|
|
|
|
function hideInitialPosePreview() {
|
|
if (posePreviewModel) posePreviewModel.visible = false;
|
|
if (initialPoseDirectionArrow) initialPoseDirectionArrow.visible = false;
|
|
}
|
|
|
|
async function sendNavigationGoal(goal) {
|
|
// Nav2 cancellation is asynchronous. Confirm any earlier goal is canceled
|
|
// before accepting the next one so it cannot keep driving to the old point.
|
|
const cancelResponse = await fetch(`${BACKEND_URL}/cancel_navigation`, { method:'POST' });
|
|
if (!cancelResponse.ok) {
|
|
const message = await cancelResponse.text().catch(() => '');
|
|
throw new Error(message || '기존 목표를 취소하지 못했습니다.');
|
|
}
|
|
navigationGoalId = null;
|
|
placeNavigationGoalMarker(goal.x, goal.y, goal.yaw);
|
|
const response = await fetch(`${BACKEND_URL}/navigate_to_pose`, {
|
|
method:'POST',
|
|
headers:{ 'Content-Type':'application/json' },
|
|
body:JSON.stringify({ ...goal, frame_id:'map' }),
|
|
});
|
|
if (!response.ok) {
|
|
const message = await response.text().catch(() => '');
|
|
throw new Error(message || 'navigation goal failed');
|
|
}
|
|
const result = await response.json().catch(() => ({ ok:true }));
|
|
navigationGoalId = result.goal_id || null;
|
|
return result;
|
|
}
|
|
|
|
function moveLocalSimulatorToInitialPose(pose) {
|
|
const requestId = `sim-pose-${Date.now()}-${Math.random().toString(36).slice(2, 8)}`;
|
|
return new Promise((resolve, reject) => {
|
|
const timeout = window.setTimeout(() => {
|
|
pendingSimulatorPoseRequests.delete(requestId);
|
|
reject(new Error('가제보 로봇 위치 적용 시간이 초과되었습니다.'));
|
|
}, 12_000);
|
|
pendingSimulatorPoseRequests.set(requestId, { resolve, reject, timeout });
|
|
window.parent.postMessage({
|
|
source: 'autonomy-slam-viewer',
|
|
type: 'simulator-pose-request',
|
|
requestId,
|
|
pose: { x: pose.x, y: pose.y, yaw: pose.yaw || 0 },
|
|
}, '*');
|
|
});
|
|
}
|
|
|
|
async function sendInitialPose(pose) {
|
|
if (REAL_RUNTIME) {
|
|
const response = await fetch(`${BACKEND_URL}/initialpose`, {
|
|
method:'POST',
|
|
headers:{ 'Content-Type':'application/json' },
|
|
body:JSON.stringify({ ...pose, frame_id:'map' }),
|
|
});
|
|
if (!response.ok) throw new Error(await response.text().catch(() => '') || 'initial pose failed');
|
|
const result = await response.json().catch(() => ({}));
|
|
// RTAB-Map first rebases ICP odometry to this nearby seed, then refines it
|
|
// against the saved map. Give its first corrected TF a short head start
|
|
// before enabling a navigation goal.
|
|
if (result.refining) await new Promise((resolve) => window.setTimeout(resolve, 750));
|
|
navigationInitialPoseSet = true;
|
|
// A real-platform click is only a localization seed. Keep the model on
|
|
// the live TF/odometry pose instead of drawing it at the clicked yaw.
|
|
syncGoalToggleAvailability();
|
|
navigationPlanSuppressed = false;
|
|
if (robotModel) robotModel.visible = true;
|
|
hideInitialPosePreview();
|
|
return;
|
|
}
|
|
// In simulation, the selected point is the robot's actual Gazebo spawn
|
|
// point as well as AMCL's initial map pose. Moving only AMCL left the lidar
|
|
// at the old spawn location and rotated the scan against the saved map.
|
|
await moveLocalSimulatorToInitialPose(pose);
|
|
await new Promise((resolve) => window.setTimeout(resolve, 200));
|
|
const response = await fetch(`${BACKEND_URL}/initialpose`, {
|
|
method:'POST',
|
|
headers:{ 'Content-Type':'application/json' },
|
|
body:JSON.stringify({ ...pose, frame_id:'map' }),
|
|
});
|
|
if (!response.ok) throw new Error(await response.text().catch(() => '') || 'initial pose failed');
|
|
navigationInitialPoseSet = true;
|
|
applyInitialNavigationPose(pose);
|
|
syncGoalToggleAvailability();
|
|
navigationPlanSuppressed = false;
|
|
if (robotModel) robotModel.visible = true;
|
|
hideInitialPosePreview();
|
|
}
|
|
|
|
function applyCameraPreset(view) {
|
|
activeCameraPreset = view || 'default';
|
|
if (view === 'top') {
|
|
const target = sdfPointToWorld(currentRobotPose.x, currentRobotPose.y, 0);
|
|
camera.position.copy(target).add(new THREE.Vector3(0, 12, 0));
|
|
camera.up.set(1, 0, 0);
|
|
controls.target.copy(target);
|
|
} else if (view === 'follow') {
|
|
updateFollowCamera(true);
|
|
return;
|
|
} else {
|
|
camera.position.copy(DEFAULT_CAMERA.position);
|
|
camera.up.set(0, 1, 0);
|
|
controls.target.copy(DEFAULT_CAMERA.target);
|
|
}
|
|
camera.lookAt(controls.target);
|
|
controls.update();
|
|
}
|
|
|
|
function updateFollowCamera(force = false) {
|
|
if (activeCameraPreset !== 'follow' && !force) return;
|
|
const desiredTarget = sdfPointToWorld(currentRobotPose.x, currentRobotPose.y, currentRobotPose.z);
|
|
const desiredPosition = desiredTarget.clone().add(new THREE.Vector3(0, 4.0, 0));
|
|
|
|
camera.up.set(1, 0, 0);
|
|
if (force) {
|
|
camera.position.copy(desiredPosition);
|
|
controls.target.copy(desiredTarget);
|
|
} else {
|
|
camera.position.lerp(desiredPosition, 0.25);
|
|
controls.target.lerp(desiredTarget, 0.25);
|
|
}
|
|
camera.lookAt(controls.target);
|
|
controls.update();
|
|
}
|
|
|
|
cameraPresetSelect.addEventListener('change', () => {
|
|
applyCameraPreset(cameraPresetSelect.value);
|
|
focusViewer();
|
|
});
|
|
|
|
renderer.domElement.addEventListener('mousemove', (event) => {
|
|
if (!initialPoseMode && !navigationGoalMode && !obstaclePlacementMode) return;
|
|
if (initialPoseDrag) return;
|
|
const pose = pickNavigationGoal(event);
|
|
if (!pose) return;
|
|
if (initialPoseMode) showInitialPosePreview(pose);
|
|
if (navigationGoalMode) setNavigationGoalPreview(pose);
|
|
if (obstaclePlacementMode) setObstaclePreview(pose);
|
|
});
|
|
|
|
renderer.domElement.addEventListener('pointerdown', (event) => {
|
|
if (EDITOR_MODE || !initialPoseMode || event.button !== 0) return;
|
|
const pose = pickNavigationGoal(event);
|
|
if (!pose) return;
|
|
|
|
initialPoseDrag = { ...pose };
|
|
controls.enabled = false;
|
|
renderer.domElement.setPointerCapture?.(event.pointerId);
|
|
showInitialPosePreview(initialPoseDrag);
|
|
event.preventDefault();
|
|
event.stopImmediatePropagation();
|
|
}, true);
|
|
|
|
renderer.domElement.addEventListener('pointermove', (event) => {
|
|
if (!initialPoseDrag) return;
|
|
const direction = pickNavigationGoal(event);
|
|
if (!direction) return;
|
|
const dx = direction.x - initialPoseDrag.x;
|
|
const dy = direction.y - initialPoseDrag.y;
|
|
if (Math.hypot(dx, dy) > 0.05) initialPoseDrag.yaw = Math.atan2(dy, dx);
|
|
showInitialPosePreview(initialPoseDrag);
|
|
event.preventDefault();
|
|
event.stopImmediatePropagation();
|
|
}, true);
|
|
|
|
renderer.domElement.addEventListener('pointerup', async (event) => {
|
|
if (!initialPoseDrag) return;
|
|
const pose = { ...initialPoseDrag };
|
|
initialPoseDrag = null;
|
|
controls.enabled = true;
|
|
renderer.domElement.releasePointerCapture?.(event.pointerId);
|
|
initialPoseMode = false;
|
|
poseToggleButton.classList.remove('active');
|
|
hideInitialPosePreview();
|
|
event.preventDefault();
|
|
event.stopImmediatePropagation();
|
|
try {
|
|
await sendInitialPose(pose);
|
|
} catch (error) {
|
|
console.warn('initial pose failed:', error);
|
|
}
|
|
}, true);
|
|
|
|
renderer.domElement.addEventListener('pointercancel', () => {
|
|
if (!initialPoseDrag) return;
|
|
initialPoseDrag = null;
|
|
controls.enabled = true;
|
|
hideInitialPosePreview();
|
|
}, true);
|
|
|
|
renderer.domElement.addEventListener('click', async (event) => {
|
|
if (EDITOR_MODE) {
|
|
const rect = renderer.domElement.getBoundingClientRect();
|
|
editorRaycaster.setFromCamera(new THREE.Vector2(
|
|
((event.clientX - rect.left) / rect.width) * 2 - 1,
|
|
-((event.clientY - rect.top) / rect.height) * 2 + 1,
|
|
), camera);
|
|
const hit = editorRaycaster.intersectObjects(editorObjects.children, false)[0];
|
|
editorSelectedId = hit?.object?.userData?.objectId || null;
|
|
attachEditorTransform();
|
|
window.parent.postMessage({ source:'autonomy-world-editor', type:'select-object', id: editorSelectedId }, '*');
|
|
return;
|
|
}
|
|
if (!navigationGoalMode && !initialPoseMode && !obstaclePlacementMode) return;
|
|
event.preventDefault();
|
|
event.stopPropagation();
|
|
const goal = pickNavigationGoal(event);
|
|
if (!goal) return;
|
|
try {
|
|
if (obstaclePlacementMode) {
|
|
const scenePoint = localGazeboNavigationUsesOdom() ? mapToOdom(goal) : goal;
|
|
const obstacle = {
|
|
id: `runtime-wall-${Date.now()}-${Math.random().toString(36).slice(2, 7)}`,
|
|
x: scenePoint.x,
|
|
y: scenePoint.y,
|
|
...(obstaclePreviewTemplate ?? createObstaclePreviewTemplate()),
|
|
};
|
|
window.parent.postMessage({ source:'autonomy-slam-viewer', type:'runtime-obstacle-add', obstacle }, '*');
|
|
addRuntimeObstacle(obstacle);
|
|
obstaclePreviewTemplate = null;
|
|
setObstaclePreview(goal);
|
|
return;
|
|
}
|
|
if (initialPoseMode) return;
|
|
await sendNavigationGoal(goal);
|
|
navigationGoalMode = false;
|
|
goalToggleButton.classList.remove('active');
|
|
hideNavigationGoalPreview();
|
|
} catch (error) {
|
|
console.warn('navigation goal failed:', error);
|
|
}
|
|
});
|
|
|
|
function applySlamState(state) {
|
|
latestSlamState = state;
|
|
resetNavigationPoseForNewSession(state);
|
|
restoreNavigationPoseFromState(state);
|
|
updateNavigationLocalizationState(state);
|
|
updateRosTopicStatus(state?.topic_status);
|
|
if (
|
|
navigationGoalMarker
|
|
&& navigationGoalId
|
|
&& state?.navigation?.goal_id === navigationGoalId
|
|
&& state.navigation.status === 'succeeded'
|
|
) {
|
|
clearNavigationGoalMarker();
|
|
navigationGoalId = null;
|
|
}
|
|
if (state?.pose || state?.pose_3d || state?.scan || state?.map || state?.point_cloud || state?.sensor_point_cloud) markVisualizationResponseReceived();
|
|
if (NAV2_VIEW_MODE === 'navigation' && state?.tf?.map_to_odom) {
|
|
navigationMapToOdom = state.tf.map_to_odom;
|
|
updateNavigationMapTransform();
|
|
}
|
|
updateNavigationPlan(state?.plan);
|
|
// Remote navigation is rendered in the map frame, matching the saved map,
|
|
// room model, AMCL pose, route and goal. Local Gazebo remains odom-based.
|
|
const visualizationPose = (REAL_RUNTIME && (SHOW_POINT_CLOUD || (NAV2_VIEW_MODE === 'navigation' && state?.pose_3d)))
|
|
? (state?.pose_3d || state?.pose)
|
|
: (NAV2_VIEW_MODE === 'navigation'
|
|
? (navigationSceneUsesMapFrame() ? (state?.pose_map || state?.pose) : state?.pose)
|
|
: (state?.pose_map || state?.pose));
|
|
if (visualizationPose) {
|
|
setRobotPose({
|
|
...visualizationPose,
|
|
steeringAngle: state?.vehicle?.steering_angle,
|
|
steeringLeftAngle: state?.vehicle?.steering_left_angle,
|
|
steeringRightAngle: state?.vehicle?.steering_right_angle,
|
|
motion: state?.pose ?? visualizationPose,
|
|
});
|
|
}
|
|
if (state?.scan) {
|
|
latestScanMetadata = state.scan;
|
|
if (latestScanRanges) {
|
|
const scan = { ...latestScanMetadata, ranges: latestScanRanges };
|
|
drawLaserScan(scan);
|
|
state = { ...state, scan };
|
|
latestSlamState = state;
|
|
}
|
|
}
|
|
if (state?.map) updateMiniMapMetadata(state.map);
|
|
drawMiniMap(state);
|
|
downloadMapButton.disabled = !latestMiniMapGrid?.data?.length;
|
|
}
|
|
|
|
function connectSlamSocket() {
|
|
slamSocket = new WebSocket(BACKEND_WS);
|
|
slamSocket.binaryType = 'arraybuffer';
|
|
slamSocket.addEventListener('message', async (event) => {
|
|
if (await applySocketBinaryPayload(event.data)) {
|
|
return;
|
|
}
|
|
try {
|
|
applySlamState(JSON.parse(event.data));
|
|
} catch (err) {
|
|
console.warn('invalid slam websocket payload:', err);
|
|
}
|
|
});
|
|
slamSocket.addEventListener('close', () => {
|
|
console.warn(`SLAM websocket disconnected. reconnecting... ${BACKEND_WS}`);
|
|
setTimeout(connectSlamSocket, 1000);
|
|
});
|
|
slamSocket.addEventListener('error', () => {
|
|
slamSocket.close();
|
|
});
|
|
|
|
connectControlSocket();
|
|
}
|
|
|
|
async function loadInitialSlamState() {
|
|
try {
|
|
const response = await fetch(`${BACKEND_URL}/state`);
|
|
if (!response.ok) return;
|
|
applySlamState(await response.json());
|
|
} catch (_) {
|
|
// The websocket reconnect path remains the fallback while the bridge starts.
|
|
}
|
|
}
|
|
|
|
void loadInitialSlamState();
|
|
if (SHOW_POINT_CLOUD) {
|
|
void refreshPointCloud();
|
|
pointCloudRefreshTimer = window.setInterval(() => void refreshPointCloud(), 1000);
|
|
}
|
|
if (SHOW_SENSOR_POINT_CLOUD && navigationSensorVisible) {
|
|
void refreshSensorPointCloud();
|
|
sensorPointCloudRefreshTimer = window.setInterval(() => void refreshSensorPointCloud(), 750);
|
|
}
|
|
if (ENABLE_CAMERA_PREVIEW && REAL_RUNTIME) {
|
|
void refreshCameraFrame();
|
|
cameraFrameRefreshTimer = window.setInterval(() => void refreshCameraFrame(), 200);
|
|
}
|
|
|
|
window.addEventListener('beforeunload', () => {
|
|
if (pointCloudRefreshTimer) window.clearInterval(pointCloudRefreshTimer);
|
|
if (sensorPointCloudRefreshTimer) window.clearInterval(sensorPointCloudRefreshTimer);
|
|
if (miniMapRenderTimer) window.clearTimeout(miniMapRenderTimer);
|
|
if (cameraFrameRefreshTimer) window.clearInterval(cameraFrameRefreshTimer);
|
|
if (cameraFrameObjectUrl) URL.revokeObjectURL(cameraFrameObjectUrl);
|
|
});
|
|
|
|
function connectControlSocket() {
|
|
if (!CONTROL_WS) return;
|
|
controlSocket = new WebSocket(CONTROL_WS);
|
|
controlSocket.addEventListener('close', () => {
|
|
setTimeout(connectControlSocket, 1000);
|
|
});
|
|
controlSocket.addEventListener('error', () => {
|
|
controlSocket.close();
|
|
});
|
|
}
|
|
|
|
let maxLinear = 0.22;
|
|
let maxAngular = 2.84;
|
|
let activeMoveKey = null;
|
|
let teleopInterval = null;
|
|
let singleMoveStopTimer = null;
|
|
const TELEOP_COMMAND_INTERVAL_MS = 120;
|
|
const SINGLE_MOVE_DURATION_MS = 120;
|
|
const LATCHED_ROTATION_SPEED_SCALE = 0.25;
|
|
const MKMINI_GAZEBO_WHEELBASE_METERS = 0.6;
|
|
const MKMINI_GAZEBO_MAX_STEERING_RADIANS = 0.5;
|
|
let cmdVelRequestInFlight = false;
|
|
let queuedCmdVel = null;
|
|
let mkMiniTeleopSteeringAngle = 0;
|
|
const moveBindings = {
|
|
u: [ 1, 1], i: [ 1, 0], o: [ 1, -1],
|
|
j: [ 0, 1], k: [ 0, 0], l: [ 0, -1],
|
|
m: [-1, -1], ',': [-1, 0], '.': [-1, 1],
|
|
};
|
|
const speedBindings = {
|
|
q: [1.1, 1.1], z: [0.9, 0.9],
|
|
w: [1.1, 1.0], x: [0.9, 1.0],
|
|
e: [1.0, 1.1], c: [1.0, 0.9],
|
|
};
|
|
|
|
function canUseTeleop(event) {
|
|
if (REAL_RUNTIME) return false;
|
|
if (!document.hasFocus()) return false;
|
|
const tag = event.target?.tagName?.toLowerCase();
|
|
return !['input', 'textarea', 'select', 'button'].includes(tag);
|
|
}
|
|
|
|
function flushQueuedCmdVel() {
|
|
if (cmdVelRequestInFlight || !queuedCmdVel) return;
|
|
const command = queuedCmdVel;
|
|
queuedCmdVel = null;
|
|
cmdVelRequestInFlight = true;
|
|
const controller = new AbortController();
|
|
const timeout = window.setTimeout(() => controller.abort(), 800);
|
|
|
|
fetch(`${BACKEND_URL}/cmd_vel`, {
|
|
method:'POST',
|
|
headers:{ 'Content-Type':'application/json' },
|
|
body:JSON.stringify(command),
|
|
signal:controller.signal,
|
|
})
|
|
.catch((error) => {
|
|
if (error?.name !== 'AbortError') console.warn(error);
|
|
})
|
|
.finally(() => {
|
|
window.clearTimeout(timeout);
|
|
cmdVelRequestInFlight = false;
|
|
flushQueuedCmdVel();
|
|
});
|
|
}
|
|
|
|
function sendCmdVel(linear, angular) {
|
|
const payload = { type:'cmd_vel', linear, angular };
|
|
if (controlSocket?.readyState === WebSocket.OPEN) {
|
|
controlSocket.send(JSON.stringify(payload));
|
|
return;
|
|
}
|
|
if (!DIRECT_BRIDGE && slamSocket?.readyState === WebSocket.OPEN) {
|
|
slamSocket.send(JSON.stringify(payload));
|
|
return;
|
|
}
|
|
queuedCmdVel = { linear, angular };
|
|
flushQueuedCmdVel();
|
|
}
|
|
|
|
function setSimulatorMkMiniSteering(angle) {
|
|
mkMiniTeleopSteeringAngle = THREE.MathUtils.clamp(
|
|
Number(angle) || 0,
|
|
-MKMINI_GAZEBO_MAX_STEERING_RADIANS,
|
|
MKMINI_GAZEBO_MAX_STEERING_RADIANS,
|
|
);
|
|
|
|
const fallbackToTwistSteering = () => {
|
|
if (Math.abs(mkMiniTeleopSteeringAngle) < 0.001) return;
|
|
// The Gazebo tricycle plugin consumes angular.z as its steering angle.
|
|
sendCmdVel(0, mkMiniTeleopSteeringAngle);
|
|
};
|
|
|
|
fetch(`${BACKEND_URL}/steering`, {
|
|
method: 'POST',
|
|
headers: { 'Content-Type': 'application/json' },
|
|
body: JSON.stringify({ angle: mkMiniTeleopSteeringAngle }),
|
|
})
|
|
.then((response) => {
|
|
if (!response.ok) fallbackToTwistSteering();
|
|
})
|
|
.catch((error) => {
|
|
console.warn('[teleop] steering update failed', error);
|
|
fallbackToTwistSteering();
|
|
});
|
|
}
|
|
|
|
function stopRobot() {
|
|
if (teleopInterval) {
|
|
window.clearInterval(teleopInterval);
|
|
teleopInterval = null;
|
|
}
|
|
if (singleMoveStopTimer) {
|
|
window.clearTimeout(singleMoveStopTimer);
|
|
singleMoveStopTimer = null;
|
|
}
|
|
sendCmdVel(0, 0);
|
|
}
|
|
|
|
function sendActiveMoveCommand() {
|
|
if (!activeMoveKey) return;
|
|
const [linearSign, angularSign] = moveBindings[activeMoveKey];
|
|
if (linearSign === 0 && angularSign === 0) return;
|
|
|
|
if (USE_MKMINI_MODEL && !REAL_RUNTIME) {
|
|
if (activeMoveKey === 'j' || activeMoveKey === 'l') {
|
|
setSimulatorMkMiniSteering(angularSign * MKMINI_GAZEBO_MAX_STEERING_RADIANS);
|
|
return;
|
|
}
|
|
if (linearSign !== 0 && Math.abs(mkMiniTeleopSteeringAngle) > 0.001) {
|
|
const linear = linearSign * maxLinear;
|
|
// Gazebo's tricycle drive consumes angular.z as the front steering
|
|
// angle. Keep this identical to the independently rendered front tyres.
|
|
sendCmdVel(linear, mkMiniTeleopSteeringAngle);
|
|
return;
|
|
}
|
|
}
|
|
|
|
const angularSpeed = linearSign === 0
|
|
? maxAngular * LATCHED_ROTATION_SPEED_SCALE
|
|
: maxAngular;
|
|
sendCmdVel(linearSign * maxLinear, angularSign * angularSpeed);
|
|
}
|
|
|
|
function startActiveMoveCommand() {
|
|
sendActiveMoveCommand();
|
|
if (teleopInterval) return;
|
|
teleopInterval = window.setInterval(sendActiveMoveCommand, TELEOP_COMMAND_INTERVAL_MS);
|
|
}
|
|
|
|
function sendSingleMoveCommand(key) {
|
|
const [linearSign, angularSign] = moveBindings[key];
|
|
activeMoveKey = null;
|
|
stopRobot();
|
|
sendCmdVel(linearSign * maxLinear, angularSign * maxAngular);
|
|
singleMoveStopTimer = window.setTimeout(() => {
|
|
singleMoveStopTimer = null;
|
|
sendCmdVel(0, 0);
|
|
}, SINGLE_MOVE_DURATION_MS);
|
|
}
|
|
|
|
window.addEventListener('keydown', (event) => {
|
|
if (!canUseTeleop(event)) return;
|
|
const key = event.key.toLowerCase();
|
|
|
|
if (event.repeat) {
|
|
if (moveBindings[key]) event.preventDefault();
|
|
return;
|
|
}
|
|
|
|
if (speedBindings[key]) {
|
|
event.preventDefault();
|
|
const [linearScale, angularScale] = speedBindings[key];
|
|
maxLinear = Math.max(0.01, maxLinear * linearScale);
|
|
maxAngular = Math.max(0.1, maxAngular * angularScale);
|
|
return;
|
|
}
|
|
|
|
if (!moveBindings[key]) return;
|
|
event.preventDefault();
|
|
if (key === 'k') {
|
|
activeMoveKey = null;
|
|
if (USE_MKMINI_MODEL && !REAL_RUNTIME) setSimulatorMkMiniSteering(0);
|
|
else mkMiniTeleopSteeringAngle = 0;
|
|
stopRobot();
|
|
return;
|
|
}
|
|
if (USE_MKMINI_MODEL && !REAL_RUNTIME && (key === 'j' || key === 'l')) {
|
|
activeMoveKey = key;
|
|
sendActiveMoveCommand();
|
|
activeMoveKey = null;
|
|
return;
|
|
}
|
|
if (!['i', ',', 'j', 'l'].includes(key)) {
|
|
sendSingleMoveCommand(key);
|
|
return;
|
|
}
|
|
activeMoveKey = key;
|
|
startActiveMoveCommand();
|
|
});
|
|
|
|
window.addEventListener('keyup', (event) => {
|
|
if (!canUseTeleop(event)) return;
|
|
const key = event.key.toLowerCase();
|
|
if (moveBindings[key]) event.preventDefault();
|
|
});
|
|
|
|
window.addEventListener('blur', () => {
|
|
if (!activeMoveKey) return;
|
|
activeMoveKey = null;
|
|
stopRobot();
|
|
});
|
|
|
|
// A static Gazebo model can contain assets that the browser cannot render.
|
|
// Keep the ROS stream available even when the decorative world mesh fails.
|
|
const worldSceneTasks = CUSTOM_WORLD || SAVED_GAZEBO_MAP_WORLD || REAL_RUNTIME
|
|
? []
|
|
: [buildFromSdf().catch((error) => console.warn('world model render failed', error))];
|
|
const remoteNavigationSceneTasks = [];
|
|
Promise.all(EDITOR_MODE
|
|
? worldSceneTasks
|
|
: [...worldSceneTasks, ...remoteNavigationSceneTasks, (USE_MKMINI_MODEL ? loadMkMiniModel : loadBurgerModel)({ ...DEFAULT_SIMULATION_SPAWN, z:0.01, roll:0, pitch:0, yaw:0 })]
|
|
).then(() => {
|
|
if (EDITOR_MODE) {
|
|
window.parent.postMessage({ source:'autonomy-world-editor', type:'ready' }, '*');
|
|
return;
|
|
}
|
|
void refreshRuntimeLayout();
|
|
connectSlamSocket();
|
|
})
|
|
.catch(err => {
|
|
console.error(err);
|
|
});
|
|
|
|
window.addEventListener('resize', () => {
|
|
camera.aspect = innerWidth / innerHeight;
|
|
camera.updateProjectionMatrix();
|
|
renderer.setSize(innerWidth, innerHeight);
|
|
});
|
|
function animate(){
|
|
requestAnimationFrame(animate);
|
|
updateSmoothedRobotPose();
|
|
updateMkMiniFrontWheelSteering();
|
|
controls.update();
|
|
renderer.render(scene, camera);
|
|
}
|
|
animate();
|
|
</script>
|
|
</body>
|
|
</html>
|