add sim mode..

This commit is contained in:
2026-08-20 13:22:27 +09:00
parent f29b44da07
commit e1d7b7d532
20 changed files with 1079 additions and 14828 deletions
+390 -24
View File
@@ -284,6 +284,52 @@ type SavedMapArtifactDownload = {
url: string
}
type PointCloudVoxel = {
maxZ: number
samples: number
}
type PointCloudLayoutCache = {
source: {
fileName: string
size: number
modifiedAtMs: number
}
layout: WorldLayout
}
type PgmImage = {
width: number
height: number
maxValue: number
pixels: Uint16Array
}
type PgmMapMetadata = {
resolution: number
originX: number
originY: number
negate: boolean
occupiedThreshold: number
}
type MapCollisionRectangle = {
startX: number
endX: number
startY: number
endY: number
}
// Build the Gazebo collision layer from the real LiDAR's horizontal sensing
// band. This rejects floor and ceiling points, then extrudes each occupied
// voxel down to the ground for a stable 2D navigation obstacle.
const pointCloudMinObstacleHeightMeters = 0.25
const pointCloudMaxObstacleHeightMeters = 1.2
const maxPointCloudCollisionObjects = 1_200
const pointCloudVoxelSizes = [0.16, 0.2, 0.25, 0.32, 0.4, 0.5]
const mapCollisionHeightMeters = 1.2
const maxMapCollisionObjects = 900
function defaultWorldLayout(): WorldLayout {
return {
version: 1,
@@ -335,6 +381,326 @@ function safeSavedMapFileName(input: string): string {
return fileName
}
function savedMapPointCloudLayoutFileName(databaseFileName: string): string {
return safeSavedMapFileName(databaseFileName.replace(/\.db$/i, '.gazebo-3d-layout.json'))
}
function savedMapPointCloudLayoutCacheFileName(databaseFileName: string): string {
return safeSavedMapFileName(databaseFileName.replace(/\.db$/i, '.gazebo-3d-layout-cache.json'))
}
function savedMapCollisionLayoutFileName(yamlFileName: string): string {
return safeSavedMapFileName(yamlFileName.replace(/\.yaml$/i, '.gazebo-layout.json'))
}
async function writeJsonAtomically(path: string, value: unknown): Promise<void> {
const tempPath = `${path}.tmp-${process.pid}-${Date.now()}-${Math.random().toString(16).slice(2)}`
await writeFile(tempPath, `${JSON.stringify(value, null, 2)}\n`, 'utf8')
await rename(tempPath, path)
}
function pgmWhitespace(byte: number | undefined): boolean {
return byte === 9 || byte === 10 || byte === 13 || byte === 32
}
function readPgmImage(contents: Buffer): PgmImage {
let offset = 0
const nextToken = (): string => {
while (offset < contents.length) {
if (contents[offset] === 35) {
while (offset < contents.length && contents[offset] !== 10) offset += 1
} else if (pgmWhitespace(contents[offset])) {
offset += 1
} else break
}
const start = offset
while (offset < contents.length && !pgmWhitespace(contents[offset]) && contents[offset] !== 35) offset += 1
return contents.subarray(start, offset).toString('ascii')
}
const format = nextToken()
const width = Number(nextToken())
const height = Number(nextToken())
const maxValue = Number(nextToken())
if (!['P2', 'P5'].includes(format) || !Number.isInteger(width) || !Number.isInteger(height)
|| width <= 0 || height <= 0 || width * height > 4_000_000
|| !Number.isInteger(maxValue) || maxValue <= 0 || maxValue > 65_535) {
throw new Error('저장된 PGM 맵 형식이 올바르지 않습니다.')
}
const pixels = new Uint16Array(width * height)
if (format === 'P2') {
for (let index = 0; index < pixels.length; index += 1) {
const value = Number(nextToken())
if (!Number.isInteger(value) || value < 0 || value > maxValue) throw new Error('저장된 PGM 맵 픽셀 데이터가 올바르지 않습니다.')
pixels[index] = value
}
return { width, height, maxValue, pixels }
}
if (!pgmWhitespace(contents[offset])) throw new Error('저장된 PGM 맵 헤더가 올바르지 않습니다.')
if (contents[offset] === 13 && contents[offset + 1] === 10) offset += 2
else offset += 1
const bytesPerPixel = maxValue < 256 ? 1 : 2
if (contents.length - offset < pixels.length * bytesPerPixel) throw new Error('저장된 PGM 맵 데이터가 불완전합니다.')
for (let index = 0; index < pixels.length; index += 1) {
pixels[index] = bytesPerPixel === 1
? contents[offset + index]
: (contents[offset + index * 2] << 8) | contents[offset + index * 2 + 1]
}
return { width, height, maxValue, pixels }
}
function readPgmMapMetadata(contents: string): PgmMapMetadata {
const scalar = (key: string, fallback: number): number => {
const match = contents.match(new RegExp(`^\\s*${key}\\s*:\\s*([^#\\r\\n]+)`, 'mi'))
const value = Number(match?.[1]?.trim())
return Number.isFinite(value) ? value : fallback
}
const origin = contents.match(/^\s*origin\s*:\s*\[\s*([-+\d.eE]+)\s*,\s*([-+\d.eE]+)/mi)
const originX = Number(origin?.[1])
const originY = Number(origin?.[2])
const resolution = scalar('resolution', NaN)
if (!Number.isFinite(resolution) || resolution <= 0 || !Number.isFinite(originX) || !Number.isFinite(originY)) {
throw new Error('저장된 맵 YAML의 해상도 또는 원점 정보가 올바르지 않습니다.')
}
return { resolution, originX, originY, negate: scalar('negate', 0) !== 0, occupiedThreshold: Math.min(1, Math.max(0, scalar('occupied_thresh', 0.65))) }
}
function mapCollisionRectangles(image: PgmImage, metadata: PgmMapMetadata, scale: number): MapCollisionRectangle[] {
const columns = Math.ceil(image.width / scale)
const rows = Math.ceil(image.height / scale)
const occupied = (x: number, y: number): boolean => {
const endY = Math.min(image.height, y + scale)
const endX = Math.min(image.width, x + scale)
for (let sourceY = y; sourceY < endY; sourceY += 1) {
for (let sourceX = x; sourceX < endX; sourceX += 1) {
const value = image.pixels[sourceY * image.width + sourceX] / image.maxValue
if ((metadata.negate ? value : 1 - value) >= metadata.occupiedThreshold) return true
}
}
return false
}
const rectangles: MapCollisionRectangle[] = []
let activeRuns = new Map<string, MapCollisionRectangle>()
for (let row = 0; row < rows; row += 1) {
const nextRuns = new Map<string, MapCollisionRectangle>()
let column = 0
while (column < columns) {
if (!occupied(column * scale, row * scale)) { column += 1; continue }
const startColumn = column
while (column < columns && occupied(column * scale, row * scale)) column += 1
const key = `${startColumn}:${column}`
const active = activeRuns.get(key)
nextRuns.set(key, active ? { ...active, endY: row + 1 } : { startX: startColumn, endX: column, startY: row, endY: row + 1 })
}
activeRuns.forEach((rectangle, key) => { if (!nextRuns.has(key)) rectangles.push(rectangle) })
activeRuns = nextRuns
}
activeRuns.forEach((rectangle) => rectangles.push(rectangle))
return rectangles
}
function createMapCollisionLayout(yamlContents: string, pgmContents: Buffer): WorldLayout {
const image = readPgmImage(pgmContents)
const metadata = readPgmMapMetadata(yamlContents)
let scale = 1
let rectangles = mapCollisionRectangles(image, metadata, scale)
while (rectangles.length > maxMapCollisionObjects && scale < 8) { scale *= 2; rectangles = mapCollisionRectangles(image, metadata, scale) }
if (rectangles.length > maxMapCollisionObjects) throw new Error('저장된 맵의 장애물이 너무 많아 시뮬레이션 충돌 환경으로 변환할 수 없습니다.')
const objects = rectangles.map((rectangle, index): WorldLayoutObject => {
const minX = metadata.originX + rectangle.startX * scale * metadata.resolution
const maxX = metadata.originX + Math.min(image.width, rectangle.endX * scale) * metadata.resolution
const minY = metadata.originY + (image.height - Math.min(image.height, rectangle.endY * scale)) * metadata.resolution
const maxY = metadata.originY + (image.height - rectangle.startY * scale) * metadata.resolution
return { id: `saved-map-obstacle-${index + 1}`, type: 'box', position: [(minX + maxX) / 2, (minY + maxY) / 2, mapCollisionHeightMeters / 2], rotation: [0, 0, 0], size: [Math.max(metadata.resolution, maxX - minX), Math.max(metadata.resolution, maxY - minY), mapCollisionHeightMeters] }
})
return normalizeWorldLayout({ version: 1, bounds: { width: 40, depth: 40 }, objects })
}
async function generateWorkspaceMapCollisionLayout(workspaceRoot: string | null | undefined, yamlPath: string, pgmPath: string): Promise<WorldLayout> {
const root = expandUserPath(workspaceRoot)
if (!root) throw new Error('워크스페이스 경로가 설정되어 있지 않습니다.')
const yamlFileName = savedMapArtifactFileName('yaml', yamlPath)
const pgmFileName = savedMapArtifactFileName('pgm', pgmPath)
const mapsDirectory = resolve(root, 'maps')
const yamlFilePath = resolve(mapsDirectory, yamlFileName)
const pgmFilePath = resolve(mapsDirectory, pgmFileName)
if (!yamlFilePath.startsWith(`${mapsDirectory}/`) || !pgmFilePath.startsWith(`${mapsDirectory}/`)) throw new Error('저장된 맵 경로가 올바르지 않습니다.')
const layout = createMapCollisionLayout(await readFile(yamlFilePath, 'utf8'), await readFile(pgmFilePath))
await mkdir(mapsDirectory, { recursive: true })
await writeJsonAtomically(resolve(mapsDirectory, savedMapCollisionLayoutFileName(yamlFileName)), layout)
await writeJsonAtomically(worldLayoutPath(root), layout)
return layout
}
function readExportedPointCloud(plyContents: Buffer): Array<[number, number, number]> {
const headerEnd = plyContents.indexOf(Buffer.from('end_header\n'))
if (headerEnd < 0 || headerEnd > 32_768) throw new Error('RTAB-Map 포인트 클라우드 헤더가 올바르지 않습니다.')
const headerLength = headerEnd + 'end_header\n'.length
const header = plyContents.subarray(0, headerLength).toString('ascii')
if (!header.includes('format binary_little_endian 1.0')) {
throw new Error('RTAB-Map 포인트 클라우드 형식을 읽을 수 없습니다.')
}
const vertexCount = Number(header.match(/^element vertex\s+(\d+)\s*$/m)?.[1])
// rtabmap-export produces x/y/z followed by five float fields.
const stride = 32
if (!Number.isInteger(vertexCount) || vertexCount <= 0 || plyContents.length < headerLength + vertexCount * stride) {
throw new Error('RTAB-Map 포인트 클라우드 데이터가 불완전합니다.')
}
const points: Array<[number, number, number]> = []
for (let index = 0; index < vertexCount; index += 1) {
const offset = headerLength + index * stride
const x = plyContents.readFloatLE(offset)
const y = plyContents.readFloatLE(offset + 4)
const z = plyContents.readFloatLE(offset + 8)
if (Number.isFinite(x) && Number.isFinite(y) && Number.isFinite(z)) points.push([x, y, z])
}
return points
}
function createPointCloudCollisionLayout(points: Array<[number, number, number]>): WorldLayout {
const obstaclePoints = points.filter(([, , z]) => (
z >= pointCloudMinObstacleHeightMeters && z <= pointCloudMaxObstacleHeightMeters
))
if (!obstaclePoints.length) throw new Error('3D SLAM 맵에서 Gazebo 장애물로 변환할 포인트를 찾지 못했습니다.')
let voxels = new Map<string, PointCloudVoxel>()
let voxelSize = pointCloudVoxelSizes[pointCloudVoxelSizes.length - 1]
for (const candidateSize of pointCloudVoxelSizes) {
const nextVoxels = new Map<string, PointCloudVoxel>()
obstaclePoints.forEach(([x, y, z]) => {
const gridX = Math.floor(x / candidateSize)
const gridY = Math.floor(y / candidateSize)
const key = `${gridX}:${gridY}`
const current = nextVoxels.get(key)
if (current) {
current.maxZ = Math.max(current.maxZ, z)
current.samples += 1
} else {
nextVoxels.set(key, { maxZ: z, samples: 1 })
}
})
voxels = nextVoxels
voxelSize = candidateSize
if (voxels.size <= maxPointCloudCollisionObjects) break
}
if (voxels.size > maxPointCloudCollisionObjects) {
throw new Error('3D SLAM 맵의 장애물이 너무 많아 Gazebo 충돌 환경으로 변환할 수 없습니다.')
}
let minX = Infinity
let minY = Infinity
let maxX = -Infinity
let maxY = -Infinity
const objects = Array.from(voxels.entries())
.filter(([, voxel]) => voxel.samples >= 2)
.map(([key, voxel], index): WorldLayoutObject => {
const [gridX, gridY] = key.split(':').map(Number)
const centerX = (gridX + 0.5) * voxelSize
const centerY = (gridY + 0.5) * voxelSize
const floorZ = 0
const ceilingZ = Math.min(pointCloudMaxObstacleHeightMeters, voxel.maxZ + voxelSize / 2)
const height = Math.max(voxelSize, ceilingZ)
minX = Math.min(minX, centerX - voxelSize / 2)
minY = Math.min(minY, centerY - voxelSize / 2)
maxX = Math.max(maxX, centerX + voxelSize / 2)
maxY = Math.max(maxY, centerY + voxelSize / 2)
return {
id: `saved-3d-map-voxel-${index + 1}`,
type: 'box',
position: [centerX, centerY, floorZ + height / 2],
rotation: [0, 0, 0],
size: [voxelSize, voxelSize, height]
}
})
if (!objects.length || !Number.isFinite(minX) || !Number.isFinite(minY) || !Number.isFinite(maxX) || !Number.isFinite(maxY)) {
throw new Error('3D SLAM 맵의 유효한 장애물을 생성하지 못했습니다.')
}
const padding = 2
return normalizeWorldLayout({
version: 1,
bounds: {
width: Math.max(10, maxX - minX + padding * 2),
depth: Math.max(10, maxY - minY + padding * 2)
},
objects
})
}
async function ensureSlamRosBridgeImage(): Promise<void> {
const exists = await runCommand('docker', ['image', 'inspect', slamRosBridgeImage], undefined, undefined, 10_000)
.then(() => true)
.catch(() => false)
if (!exists) await ensureBridgeImage(slamRosBridgeImage, 'resources/slam-ros-bridge')
}
async function exportWorkspaceMapPointCloud(workspaceRoot: string, databaseFileName: string): Promise<Buffer> {
const exportDirectoryName = `.gazebo-3d-export-${process.pid}-${Date.now()}-${Math.random().toString(16).slice(2)}`
const mapsDirectory = resolve(workspaceRoot, 'maps')
const exportDirectory = resolve(mapsDirectory, exportDirectoryName)
const exportedPly = resolve(exportDirectory, 'gazebo-map_cloud.ply')
await mkdir(exportDirectory, { recursive: true })
try {
await ensureSlamRosBridgeImage()
const userId = typeof process.getuid === 'function' ? process.getuid() : 1000
const groupId = typeof process.getgid === 'function' ? process.getgid() : 1000
await runCommand('docker', [
'run', '--rm', '--user', `${userId}:${groupId}`,
'-v', `${workspaceRoot}:/workspace`,
'--entrypoint', 'bash',
slamRosBridgeImage,
'-lc', [
'source /opt/ros/humble/setup.bash',
`rtabmap-export --cloud --scan --voxel 0.08 --output gazebo-map --output_dir /workspace/maps/${exportDirectoryName} /workspace/maps/${databaseFileName}`
].join(' && ')
], undefined, undefined, 180_000)
return await readFile(exportedPly)
} finally {
await rm(exportDirectory, { recursive: true, force: true }).catch(() => undefined)
}
}
async function generateWorkspacePointCloudCollisionLayout(
workspaceRoot: string | null | undefined,
databasePath: string,
): Promise<WorldLayout> {
const root = expandUserPath(workspaceRoot)
if (!root) throw new Error('워크스페이스 경로가 설정되어 있지 않습니다.')
const databaseFileName = savedMapArtifactFileName('database', databasePath)
const mapsDirectory = resolve(root, 'maps')
const databaseFilePath = resolve(mapsDirectory, databaseFileName)
if (!databaseFilePath.startsWith(`${mapsDirectory}/`)) throw new Error('저장된 3D 맵 경로가 올바르지 않습니다.')
const layoutPath = resolve(mapsDirectory, savedMapPointCloudLayoutFileName(databaseFileName))
const cachePath = resolve(mapsDirectory, savedMapPointCloudLayoutCacheFileName(databaseFileName))
const sourceStat = await stat(databaseFilePath)
try {
const cached = JSON.parse(await readFile(cachePath, 'utf8')) as Partial<PointCloudLayoutCache>
if (cached.source?.fileName === databaseFileName
&& cached.source.size === sourceStat.size
&& cached.source.modifiedAtMs === sourceStat.mtimeMs
&& cached.layout) {
const layout = normalizeWorldLayout(cached.layout)
await writeJsonAtomically(worldLayoutPath(root), layout)
return layout
}
} catch {
// Create the cached layout below when it does not exist or is stale.
}
const layout = createPointCloudCollisionLayout(readExportedPointCloud(
await exportWorkspaceMapPointCloud(root, databaseFileName)
))
await mkdir(mapsDirectory, { recursive: true })
await writeJsonAtomically(layoutPath, layout)
await writeJsonAtomically(cachePath, {
source: { fileName: databaseFileName, size: sourceStat.size, modifiedAtMs: sourceStat.mtimeMs },
layout,
} satisfies PointCloudLayoutCache)
await writeJsonAtomically(worldLayoutPath(root), layout)
return layout
}
function savedMapArtifactFileName(name: SavedMapArtifactName, path: string): string {
const fileName = safeSavedMapFileName(path)
const expectedExtension = name === 'yaml' ? '.yaml' : name === 'pgm' ? '.pgm' : '.db'
@@ -1600,29 +1966,15 @@ async function startRemoteVisualizationBridge(
.map(([key, value]) => `-e ${quoteShellSingle(`${key}=${value}`)}`)
.join(' ')
let image = resolveVisualizationBridgeImage(stack, environmentProfile?.bridge, environmentProfile?.runtimeImages)
if (stack === 'slam3d' && image === slamRosBridgeImage) {
const jetson3dMapSaveBridgeInstalled = await runSshOutput(
target,
'docker image inspect slam-ros-bridge:jetson_orin-3d-map-save >/dev/null 2>&1 && printf yes || true',
10_000,
)
if (jetson3dMapSaveBridgeInstalled.trim() === 'yes') {
image = 'slam-ros-bridge:jetson_orin-3d-map-save'
} else {
const jetson3dBridgeInstalled = await runSshOutput(
target,
'docker image inspect slam-ros-bridge:jetson_orin-3d >/dev/null 2>&1 && printf yes || true',
10_000,
)
if (jetson3dBridgeInstalled.trim() === 'yes') image = 'slam-ros-bridge:jetson_orin-3d'
}
} else if (stack !== 'autoware' && image === slamRosBridgeImage) {
const jetsonBridgeInstalled = await runSshOutput(
target,
'docker image inspect slam-ros-bridge:jetson_orin >/dev/null 2>&1 && printf yes || true',
10_000,
)
if (jetsonBridgeInstalled.trim() === 'yes') image = 'slam-ros-bridge:jetson_orin'
const remoteJetsonBridgeImage = environmentProfile?.runtimeImages
?.find((artifact) => (
/^slam-ros-bridge:jetson_orin(?:[-:].*)?$/i.test(artifact?.image?.trim() ?? '')
&& Boolean(artifact?.url?.trim())
))
?.image
?.trim()
if (stack !== 'autoware' && image === slamRosBridgeImage && remoteJetsonBridgeImage) {
image = remoteJetsonBridgeImage
}
const imageArtifact = environmentProfile?.runtimeImages?.find((artifact) => artifact?.image?.trim() === image)
if (imageArtifact?.url?.trim()) {
@@ -1631,11 +1983,19 @@ async function startRemoteVisualizationBridge(
`IMAGE=${quoteShellSingle(image)}`,
`URL=${quoteShellSingle(imageArtifact.url.trim())}`,
`FILE_NAME=${quoteShellSingle(safeArtifactFileName(imageArtifact.url.trim(), imageArtifact.fileName))}`,
`EXPECTED_SHA256=${quoteShellSingle(imageArtifact.checksumSha256?.trim().toLowerCase() ?? '')}`,
'if docker image inspect "$IMAGE" >/dev/null 2>&1; then exit 0; fi',
'CACHE="${XDG_CACHE_HOME:-$HOME/.cache}/autonomy-studio/artifacts"',
'mkdir -p "$CACHE"',
'ARCHIVE="$CACHE/$FILE_NAME"',
'if [ -f "$ARCHIVE" ] && [ -n "$EXPECTED_SHA256" ]; then',
' [ "$(sha256sum "$ARCHIVE" | awk \'{print $1}\')" = "$EXPECTED_SHA256" ] || rm -f "$ARCHIVE"',
'fi',
'if [ ! -f "$ARCHIVE" ]; then curl -fL "$URL" -o "$ARCHIVE"; fi',
'if [ -n "$EXPECTED_SHA256" ] && [ "$(sha256sum "$ARCHIVE" | awk \'{print $1}\')" != "$EXPECTED_SHA256" ]; then',
' echo "시각화 이미지 체크섬이 일치하지 않습니다: $FILE_NAME" >&2',
' exit 1',
'fi',
'case "$ARCHIVE" in',
' *.tar.zst|*.tzst) zstd -dc "$ARCHIVE" | docker load ;;',
' *) docker load -i "$ARCHIVE" ;;',
@@ -5415,7 +5775,7 @@ function registerLocalWorkspaceHandlers(): void {
const targetPath = worldLayoutPath(workspaceRoot)
const normalized = normalizeWorldLayout(layout)
await mkdir(dirname(targetPath), { recursive: true })
await writeFile(targetPath, `${JSON.stringify(normalized, null, 2)}\n`, 'utf8')
await writeJsonAtomically(targetPath, normalized)
return normalized
})
ipcMain.handle('local-workspace:maps:read-artifacts', (_event, workspaceRoot: string | null | undefined, artifacts: SavedMapArtifactPath[]) => (
@@ -5424,6 +5784,12 @@ function registerLocalWorkspaceHandlers(): void {
ipcMain.handle('local-workspace:maps:sync', (_event, workspaceRoot: string | null | undefined, artifacts: SavedMapArtifactDownload[]) => (
syncWorkspaceSavedMapArtifacts(workspaceRoot, artifacts)
))
ipcMain.handle('local-workspace:maps:generate-collision-layout', (_event, workspaceRoot: string | null | undefined, yamlPath: string, pgmPath: string) => (
generateWorkspaceMapCollisionLayout(workspaceRoot, yamlPath, pgmPath)
))
ipcMain.handle('local-workspace:maps:generate-point-cloud-collision-layout', (_event, workspaceRoot: string | null | undefined, databasePath: string) => (
generateWorkspacePointCloudCollisionLayout(workspaceRoot, databasePath)
))
ipcMain.handle('remote-workspace:modules:list', (_event, target: RemoteWorkspaceTarget, modules?: CourseModuleInput[] | null) => collectRemoteAvailableModules(target, modules))
ipcMain.handle('remote-workspace:ros-graph', (_event, target: RemoteWorkspaceTarget) => collectRemoteRosGraph(target))
ipcMain.handle('remote-workspace:maps:read-artifacts', (_event, target: RemoteWorkspaceTarget, artifacts: SavedMapArtifactPath[]) => (