feat: enable hybrid local and remote module execution
This commit is contained in:
@@ -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' }, '*');
|
||||
|
||||
Reference in New Issue
Block a user