feat: enable hybrid local and remote module execution

This commit is contained in:
2026-08-06 16:18:32 +09:00
parent 9cbf4ac7e9
commit 2ab7e279eb
37 changed files with 3267 additions and 744 deletions
+230 -59
View File
@@ -24,7 +24,7 @@
#miniMapToggle {
position:absolute;
right:16px;
bottom:236px;
bottom:240px;
z-index:11;
min-height:28px;
padding:0 10px;
@@ -76,6 +76,65 @@
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;
@@ -247,17 +306,19 @@
background:rgba(255,255,255,.1);
color:#fff;
}
.editor-mode #topRightControls,
.editor-mode #topRightControls,
.editor-mode #miniMap,
.editor-mode #miniMapToggle,
.editor-mode #keyGuide {
.editor-mode #keyGuide,
.editor-mode #rosTopicStatusWrap {
display:none;
}
#splatToggle { display:none; }
.nav2-slam #goalToggle, .nav2-slam #poseToggle, .nav2-slam #sensorToggle { display:none; }
.nav2-slam #goalToggle, .nav2-slam #poseToggle, .nav2-slam #sensorToggle, .nav2-slam #initToggle { display:none; }
.nav2-navigation #downloadWrap { display:none; }
.nav2-navigation #topLeftControls { display:block; }
.nav2-navigation #keyGuide { display:none; }
.nav2-navigation #rosTopicStatusWrap { bottom:16px; }
.nav2-navigation.real-runtime #splatToggle { display:inline-flex; }
.real-runtime #topLeftControls { display:none; }
.real-runtime #keyGuide { display:none; }
@@ -288,6 +349,15 @@
<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>
@@ -315,9 +385,17 @@ import * as GaussianSplats3D from '@mkkellogg/gaussian-splats-3d';
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 CUSTOM_WORLD = !REAL_RUNTIME && viewerMapKey === 'nav2-custom';
const activeWorldModel = viewerMapKey === 'nav2-turtlebot3-house' ? 'turtlebot3_house' : 'turtlebot3_world';
const MKMINI_WORLD_SPACING_SCALE = USE_MKMINI_MODEL && activeWorldModel === 'turtlebot3_world' ? 1.6 : 1;
const MKMINI_RENDER_LENGTH = 1.05;
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');
@@ -333,6 +411,10 @@ const goalToggleButton = document.getElementById('goalToggle');
const obstacleToggleButton = document.getElementById('obstacleToggle');
const initToggleButton = document.getElementById('initToggle');
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';
const SHOW_REMOTE_NAVIGATION_ROOM = REAL_RUNTIME && NAV2_VIEW_MODE === 'navigation';
@@ -347,6 +429,44 @@ const cameraPresetSelect = document.getElementById('cameraPresetSelect');
if (EDITOR_MODE) responseWaitOverlay?.classList.add('is-hidden');
let visualizationResponseReceived = EDITOR_MODE;
const ROS_TOPIC_STATUS_KEYS = NAV2_VIEW_MODE === 'navigation'
? ['scan', 'odom', 'map', 'amcl_pose', 'plan', 'cmd_vel', 'navigate_to_pose', ...(SHOW_POINT_CLOUD ? ['point_cloud'] : [])]
: ['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',
};
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;
}));
}
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;
@@ -421,7 +541,7 @@ let remoteNavigationSplatTransform = null;
let remoteNavigationSplatEnabled = false;
let remoteNavigationSplatLoading = null;
// 화면의 빨간 X축과 평행하게 오른쪽으로 미세 보정합니다.
const remoteNavigationSplatBasePosition = new THREE.Vector3(-1.7322845855746674, 1.3107954323633903, -0.8021416967574936);
const remoteNavigationSplatBasePosition = new THREE.Vector3(-1.6822845855746674, 1.3107954323633903, -0.8021416967574936);
const remoteNavigationSplatBaseQuaternion = new THREE.Quaternion(-0.10999094822602469, 0.6984998148234114, 0.6984998148234114, 0.10999094822602469);
const remoteNavigationSplatBaseScale = new THREE.Vector3(3.672624762730334, 3.672624762730334, 3.5309628207501587);
const pointCloudGeometry = SHOW_POINT_CLOUD ? new THREE.BufferGeometry() : null;
@@ -651,6 +771,13 @@ 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 the real-platform navigation scene in that frame. Local Gazebo still
// needs odom coordinates for its pose service, so its conversion remains.
function navigationSceneUsesMapFrame() {
return REAL_RUNTIME && NAV2_VIEW_MODE === 'navigation';
}
function syncGoalToggleAvailability() {
const disabled = NAV2_VIEW_MODE === 'navigation' && !navigationInitialPoseSet;
goalToggleButton.disabled = disabled;
@@ -694,6 +821,17 @@ 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,
x: pose.x * MKMINI_WORLD_SPACING_SCALE,
y: pose.y * MKMINI_WORLD_SPACING_SCALE,
});
}
function isMkMiniWorldPillar(visual) {
return MKMINI_WORLD_SPACING_SCALE > 1
&& /^(one|two|three)_/.test(visual.getAttribute('name') || '');
}
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 = '') {
@@ -754,6 +892,9 @@ async function addMesh(parent, visual, geometry) {
if (!uri) return;
const url = gazeboUriToPath(uri);
const scale = parseScale(getDirectText(meshEl, 'scale', '1 1 1'));
if (MKMINI_WORLD_SPACING_SCALE > 1 && uri.endsWith('/wall.dae')) {
scale.multiplyScalar(MKMINI_WORLD_SPACING_SCALE);
}
const material = materialFromVisual(visual);
if (/\.stl$/i.test(url)) {
@@ -801,7 +942,8 @@ function addBox(parent, visual, geometry) {
function addCylinder(parent, visual, geometry) {
const cyl = directChild(geometry, 'cylinder');
const radius = Number(getDirectText(cyl, 'radius', '0.1')) || .1;
const length = Number(getDirectText(cyl, 'length', '1')) || 1;
const sourceLength = Number(getDirectText(cyl, 'length', '1')) || 1;
const length = isMkMiniWorldPillar(visual) && sourceLength === .5 ? .8 : 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;
@@ -827,7 +969,7 @@ async function buildFromSdf() {
const model = doc.querySelector('model');
const modelGroup = new THREE.Group();
modelGroup.name = model?.getAttribute('name') || 'turtlebot3_world';
applySdfPose(modelGroup, parsePose(getDirectText(model, 'pose', '0 0 0 0 0 0')));
applyWorldSdfPose(modelGroup, parsePose(getDirectText(model, 'pose', '0 0 0 0 0 0')));
sdfRoot.add(modelGroup);
let loaded = 0;
@@ -835,19 +977,20 @@ async function buildFromSdf() {
const nestedModelGroup = new THREE.Group();
let directGeometryCount = 0;
nestedModelGroup.name = sdfModel.getAttribute('name') || 'model';
applySdfPose(nestedModelGroup, parsePose(getDirectText(sdfModel, 'pose', '0 0 0 0 0 0')));
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';
applySdfPose(linkGroup, parsePose(getDirectText(link, 'pose', '0 0 0 0 0 0')));
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'))) {
const visualGroup = new THREE.Group();
visualGroup.name = visual.getAttribute('name') || 'visual';
applySdfPose(visualGroup, parsePose(getDirectText(visual, 'pose', '0 0 0 0 0 0')));
applyWorldSdfPose(visualGroup, parsePose(getDirectText(visual, 'pose', '0 0 0 0 0 0')));
if (isMkMiniWorldPillar(visual)) visualGroup.position.z += 0.15;
linkGroup.add(visualGroup);
const geometry = directChild(visual, 'geometry');
@@ -983,7 +1126,8 @@ async function loadMkMiniModel(sdfPose = { x:0, y:0, z:0, roll:0, pitch:0, yaw:0
const initialBounds = new THREE.Box3().setFromObject(model);
const initialSize = initialBounds.getSize(new THREE.Vector3());
const largestDimension = Math.max(initialSize.x, initialSize.y, initialSize.z, 0.001);
model.scale.setScalar(0.66 / largestDimension);
// Match the Gazebo MKMini collision body's 1.05 m longest dimension.
model.scale.setScalar(MKMINI_RENDER_LENGTH / largestDimension);
model.updateMatrixWorld(true);
const normalizedBounds = new THREE.Box3().setFromObject(model);
@@ -1083,14 +1227,15 @@ async function setRemoteNavigationSplatVisible(visible) {
}
const LASER_HOLE_R = 0.14;
const LASER_OFFSET_X = REAL_RUNTIME ? 0.1 : -0.032;
const LASER_OFFSET_X = USE_MKMINI_MODEL ? 0.1 : -0.032;
const LASER_OFFSET_Y = 0;
// Calibrated to the MKMini GLB at the current display scale.
const LASER_Z = REAL_RUNTIME ? 0.5 : 0.181;
const LASER_Z = USE_MKMINI_MODEL ? 0.5 : 0.181;
// The MKMini ROS frame reports os_lidar at Z=0.6 m while the bundled GLB
// places the visible LiDAR head at approximately Z=0.5 m. Keep ROS/map
// coordinates untouched and correct only the rendered scan origin.
const MKMINI_LASER_RENDER_Z_OFFSET = 0.1;
const MKMINI_LASER_RENDER_FORWARD_OFFSET = 0.10;
function setRobotPose(pose) {
if (!pose) return;
@@ -1234,7 +1379,7 @@ function setNavigationSensorVisible(visible) {
function drawLaserScan(scan, scanPose = (
NAV2_VIEW_MODE === 'navigation'
? latestSlamState?.scan_pose
? (navigationSceneUsesMapFrame() ? latestSlamState?.scan_pose_map : latestSlamState?.scan_pose)
: latestSlamState?.scan_pose_map
) || null) {
if (NAV2_VIEW_MODE === 'navigation' && (!navigationInitialPoseSet || !navigationSensorVisible)) {
@@ -1245,8 +1390,13 @@ function drawLaserScan(scan, scanPose = (
clearLaserScan();
const sourcePose = scanPose || currentRobotPose;
const renderPose = REAL_RUNTIME && scanPose
? { ...sourcePose, z: (sourcePose.z || 0) - MKMINI_LASER_RENDER_Z_OFFSET }
const renderPose = USE_MKMINI_MODEL && scanPose
? {
...sourcePose,
x: sourcePose.x + Math.cos(sourcePose.yaw || 0) * MKMINI_LASER_RENDER_FORWARD_OFFSET,
y: sourcePose.y + Math.sin(sourcePose.yaw || 0) * MKMINI_LASER_RENDER_FORWARD_OFFSET,
z: (sourcePose.z || 0) - MKMINI_LASER_RENDER_Z_OFFSET,
}
: sourcePose;
const origin = scanPose
? new THREE.Vector3(renderPose.x, renderPose.y, renderPose.z || 0)
@@ -1268,31 +1418,24 @@ function drawLaserScan(scan, scanPose = (
const start = origin.clone().addScaledVector(dir, startD);
const point = origin.clone().addScaledVector(dir, d);
endpoints.push({ start, point, hit: valid });
linePoints.push(start.x, start.y, start.z, point.x, point.y, point.z);
// A range-max value represents open space, not an actual reflected beam.
if (valid) linePoints.push(start.x, start.y, start.z, point.x, point.y, point.z);
}
const fillPoints = [];
const openFillPoints = [];
for (let i = 0; i < endpoints.length; i++) {
const curr = endpoints[i];
const next = endpoints[(i + 1) % endpoints.length];
const targetPoints = curr.hit && next.hit ? fillPoints : openFillPoints;
targetPoints.push(curr.start.x, curr.start.y, curr.start.z);
targetPoints.push(curr.point.x, curr.point.y, curr.point.z);
targetPoints.push(next.point.x, next.point.y, next.point.z);
targetPoints.push(curr.start.x, curr.start.y, curr.start.z);
targetPoints.push(next.point.x, next.point.y, next.point.z);
targetPoints.push(next.start.x, next.start.y, next.start.z);
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);
}
const grp = new THREE.Group();
const openFillMat = new THREE.MeshBasicMaterial({
color: 0x3390ff,
transparent: true,
opacity: 0.18,
side: THREE.DoubleSide,
depthWrite: false,
});
const fillMat = new THREE.MeshBasicMaterial({
color: 0x005cff,
transparent: true,
@@ -1302,11 +1445,6 @@ function drawLaserScan(scan, scanPose = (
});
const lineMat = new THREE.LineBasicMaterial({ color: 0x3366ff, transparent: true, opacity: 0.5 });
if (openFillPoints.length) {
const geo = new THREE.BufferGeometry();
geo.setAttribute('position', new THREE.Float32BufferAttribute(openFillPoints, 3));
grp.add(new THREE.Mesh(geo, openFillMat));
}
if (fillPoints.length) {
const geo = new THREE.BufferGeometry();
geo.setAttribute('position', new THREE.Float32BufferAttribute(fillPoints, 3));
@@ -1318,7 +1456,7 @@ function drawLaserScan(scan, scanPose = (
const frontIndex = Math.max(0, Math.min(scan.ranges.length - 1, Math.round((0 - angleMin) / angleInc)));
const front = endpoints[frontIndex];
if (front) {
if (front?.hit) {
const delta = new THREE.Vector3(
front.point.x - front.start.x,
front.point.y - front.start.y,
@@ -1549,6 +1687,12 @@ function updateMiniMapMetadata(map) {
function updateNavigationMapTransform() {
if (NAV2_VIEW_MODE !== 'navigation') return;
if (navigationSceneUsesMapFrame()) {
navigationMapRoot.position.set(0, 0, 0);
navigationMapRoot.rotation.set(0, 0, 0);
updateRemoteNavigationRoomTransform();
return;
}
const transform = navigationMapToOdom || { x:0, y:0, yaw:0 };
const yaw = transform.yaw || 0;
const c = Math.cos(yaw);
@@ -1567,8 +1711,19 @@ function updateNavigationMapTransform() {
function updateRemoteNavigationRoomTransform() {
if (!remoteNavigationSplatEnabled || !remoteNavigationSplatScene || NAV2_VIEW_MODE !== 'navigation') return;
if (navigationSceneUsesMapFrame()) {
if (remoteNavigationSplatTransform?.frame === 'map') return;
remoteNavigationSplatScene.position.copy(remoteNavigationSplatBasePosition);
remoteNavigationSplatScene.quaternion.copy(remoteNavigationSplatBaseQuaternion);
remoteNavigationSplatScene.scale.copy(remoteNavigationSplatBaseScale);
remoteNavigationSplatViewer.splatMesh.updateTransforms();
remoteNavigationSplatTransform = { frame: 'map' };
return;
}
const transform = navigationMapToOdom || { x:0, y:0, yaw:0 };
if (remoteNavigationSplatTransform
&& remoteNavigationSplatTransform.frame === 'odom'
&& Math.abs(remoteNavigationSplatTransform.x - (transform.x || 0)) < 0.03
&& Math.abs(remoteNavigationSplatTransform.y - (transform.y || 0)) < 0.03
&& Math.abs(remoteNavigationSplatTransform.yaw - (transform.yaw || 0)) < 0.01) {
@@ -1600,6 +1755,7 @@ function updateRemoteNavigationRoomTransform() {
);
remoteNavigationSplatViewer.splatMesh.updateTransforms();
remoteNavigationSplatTransform = {
frame: 'odom',
x: transform.x || 0,
y: transform.y || 0,
yaw,
@@ -1656,8 +1812,10 @@ function renderNavigationOccupancyMap() {
updateNavigationMapTransform();
if (!navigationMapFocused) {
const odomCenter = mapToOdom({ x:centerX, y:centerY, yaw:originYaw });
const center = sdfPointToWorld(odomCenter.x, odomCenter.y, 0);
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);
@@ -2040,7 +2198,7 @@ function placeNavigationGoalMarker(x, y, yaw = 0) {
function updateNavigationGoalMarker() {
if (!navigationGoalMarker || !navigationGoalPose) return;
const markerPose = NAV2_VIEW_MODE === 'navigation'
const markerPose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame()
? mapToOdom(navigationGoalPose)
: navigationGoalPose;
navigationGoalMarker.position.set(markerPose.x, markerPose.y, 0.05);
@@ -2055,9 +2213,9 @@ function setNavigationGoalPreview(pose) {
);
sdfRoot.add(navigationGoalPreview);
}
const odomPose = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(pose) : pose;
navigationGoalPreview.position.set(odomPose.x, odomPose.y, 0.06);
navigationGoalPreview.rotation.z = odomPose.yaw || 0;
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;
}
@@ -2098,8 +2256,8 @@ function setObstaclePreview(pose) {
);
sdfRoot.add(obstaclePreview);
}
const odomPose = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(pose) : pose;
obstaclePreview.position.set(odomPose.x, odomPose.y, obstaclePreviewTemplate.height / 2);
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;
}
@@ -2122,8 +2280,8 @@ function updateNavigationPlan(plan) {
}
if (poses.length < 2) return;
const points = poses.map((pose) => {
const odomPose = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(pose) : pose;
return new THREE.Vector3(odomPose.x, odomPose.y, 0.07);
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),
@@ -2150,19 +2308,29 @@ function pickNavigationGoal(event) {
const worldPoint = new THREE.Vector3();
if (!navigationRaycaster.ray.intersectPlane(navigationGroundPlane, worldPoint)) return null;
const sdfPoint = worldPointToSdf(worldPoint);
const mapPoint = NAV2_VIEW_MODE === 'navigation'
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,
yaw: mapPoint.yaw || 0,
// 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' ? mapToOdom(pose) : pose;
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;
@@ -2235,7 +2403,7 @@ async function sendInitialPose(pose) {
hideInitialPosePreview();
return;
}
const gazeboPose = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(pose) : pose;
const gazeboPose = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? mapToOdom(pose) : pose;
const requestId = `initial-pose-${Date.now()}-${++initialPoseRequestSequence}`;
await new Promise((resolve, reject) => {
const timeout = window.setTimeout(() => {
@@ -2397,11 +2565,11 @@ renderer.domElement.addEventListener('click', async (event) => {
if (!goal) return;
try {
if (obstaclePlacementMode) {
const sdfPoint = NAV2_VIEW_MODE === 'navigation' ? mapToOdom(goal) : goal;
const scenePoint = NAV2_VIEW_MODE === 'navigation' && !navigationSceneUsesMapFrame() ? mapToOdom(goal) : goal;
const obstacle = {
id: `runtime-wall-${Date.now()}-${Math.random().toString(36).slice(2, 7)}`,
x: sdfPoint.x,
y: sdfPoint.y,
x: scenePoint.x,
y: scenePoint.y,
...(obstaclePreviewTemplate ?? createObstaclePreviewTemplate()),
};
window.parent.postMessage({ source:'autonomy-slam-viewer', type:'runtime-obstacle-add', obstacle }, '*');
@@ -2422,6 +2590,7 @@ renderer.domElement.addEventListener('click', async (event) => {
function applySlamState(state) {
latestSlamState = state;
updateRosTopicStatus(state?.topic_status);
if (
navigationGoalMarker
&& navigationGoalId
@@ -2437,11 +2606,13 @@ function applySlamState(state) {
updateNavigationMapTransform();
}
updateNavigationPlan(state?.plan);
// Navigation is rendered in odom so the map, robot, path and goal share
// one scene frame. SLAM keeps its map-frame pose for the live map view.
// 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 = SHOW_POINT_CLOUD
? (state?.pose_3d || state?.pose)
: (NAV2_VIEW_MODE === 'navigation' ? state?.pose : (state?.pose_map || state?.pose));
: (NAV2_VIEW_MODE === 'navigation'
? (navigationSceneUsesMapFrame() ? (state?.pose_map || state?.pose) : state?.pose)
: (state?.pose_map || state?.pose));
if (visualizationPose) setRobotPose(visualizationPose);
if (state?.scan) {
latestScanMetadata = state.scan;
@@ -2658,7 +2829,7 @@ const worldSceneTasks = CUSTOM_WORLD || REAL_RUNTIME
const remoteNavigationSceneTasks = [];
Promise.all(EDITOR_MODE
? worldSceneTasks
: [...worldSceneTasks, ...remoteNavigationSceneTasks, (REAL_RUNTIME ? loadMkMiniModel : loadBurgerModel)({ x:-2.0, y:-0.5, z:0.01, roll:0, pitch:0, yaw:0 })]
: [...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' }, '*');