feat: launch ArborFlow behavior tree studio

2026-10-24 02:02:19 +08:00 · 2026-07-21 20:09:00 +08:00
commit cb6ca3d24b
32 changed files with 9869 additions and 0 deletions
--- a/src/lib/catalog.ts
+++ b/src/lib/catalog.ts
@@ -0,0 +1,80 @@
+import {
+  GitBranch,
+  ListTree,
+  Repeat2,
+  RotateCcw,
+  ShieldCheck,
+  ShieldX,
+  Split,
+  SquareFunction,
+  TreePine,
+  Zap,
+  type LucideIcon,
+} from 'lucide-react'
+import type { BehaviorNode, NodeCategory, NodeType } from '../types'
+
+export interface NodeDefinition {
+  type: NodeType
+  label: string
+  description: string
+  category: NodeCategory
+  icon: LucideIcon
+  defaultPorts?: Record<string, string>
+}
+
+export const CATEGORY_LABELS: Record<NodeCategory, string> = {
+  control: '控制',
+  decorator: '装饰',
+  action: '动作',
+  condition: '条件',
+  subtree: '子树',
+}
+
+export const NODE_DEFINITIONS: NodeDefinition[] = [
+  { type: 'Sequence', label: 'Sequence', description: '依次执行，遇失败停止', category: 'control', icon: ListTree },
+  { type: 'Fallback', label: 'Fallback', description: '依次尝试，遇成功停止', category: 'control', icon: GitBranch },
+  { type: 'Parallel', label: 'Parallel', description: '并行执行多个子节点', category: 'control', icon: Split, defaultPorts: { success_count: '1', failure_count: '1' } },
+  { type: 'ReactiveSequence', label: 'Reactive Sequence', description: '每次 tick 从首节点重评估', category: 'control', icon: RotateCcw },
+  { type: 'ReactiveFallback', label: 'Reactive Fallback', description: '响应式回退控制', category: 'control', icon: RotateCcw },
+  { type: 'Inverter', label: 'Inverter', description: '反转成功与失败结果', category: 'decorator', icon: Repeat2 },
+  { type: 'RetryUntilSuccessful', label: 'Retry', description: '失败后重试指定次数', category: 'decorator', icon: Repeat2, defaultPorts: { num_attempts: '3' } },
+  { type: 'Repeat', label: 'Repeat', description: '重复执行指定次数', category: 'decorator', icon: Repeat2, defaultPorts: { num_cycles: '3' } },
+  { type: 'ForceSuccess', label: 'Force Success', description: '强制返回成功', category: 'decorator', icon: ShieldCheck },
+  { type: 'ForceFailure', label: 'Force Failure', description: '强制返回失败', category: 'decorator', icon: ShieldX },
+  { type: 'Action', label: 'Action', description: '调用 ROS 动作或业务逻辑', category: 'action', icon: Zap },
+  { type: 'Condition', label: 'Condition', description: '检查黑板或环境条件', category: 'condition', icon: SquareFunction },
+  { type: 'SubTree', label: 'SubTree', description: '调用另一棵行为树', category: 'subtree', icon: TreePine },
+  { type: 'AlwaysSuccess', label: 'Always Success', description: '始终返回成功', category: 'action', icon: ShieldCheck },
+  { type: 'AlwaysFailure', label: 'Always Failure', description: '始终返回失败', category: 'action', icon: ShieldX },
+]
+
+export const NODE_DEFINITION_MAP = new Map(NODE_DEFINITIONS.map((definition) => [definition.type, definition]))
+
+export function createNode(type: NodeType, position = { x: 80, y: 80 }): BehaviorNode {
+  const definition = NODE_DEFINITION_MAP.get(type)!
+  const id = `node_${crypto.randomUUID().replaceAll('-', '').slice(0, 10)}`
+  const customName = type === 'Action' ? 'MyAction' : type === 'Condition' ? 'MyCondition' : type === 'SubTree' ? 'SubTreeID' : type
+  return {
+    id,
+    type: 'behavior',
+    position,
+    data: {
+      label: customName,
+      nodeType: type,
+      category: definition.category,
+      registrationName: customName,
+      ports: { ...(definition.defaultPorts || {}) },
+      notes: '',
+      breakpoint: false,
+    },
+  }
+}
+
+export function canHaveChildren(type: NodeType) {
+  return !['Action', 'Condition', 'SubTree', 'AlwaysSuccess', 'AlwaysFailure'].includes(type)
+}
+
+export function maxChildren(type: NodeType) {
+  if (['Inverter', 'RetryUntilSuccessful', 'Repeat', 'ForceSuccess', 'ForceFailure'].includes(type)) return 1
+  return canHaveChildren(type) ? Number.POSITIVE_INFINITY : 0
+}
--- a/src/lib/demo.ts
+++ b/src/lib/demo.ts
@@ -0,0 +1,54 @@
+import type { BehaviorTreeDocument } from '../types'
+
+export function createDemoDocument(): BehaviorTreeDocument {
+  return {
+    format: 'arborflow/project',
+    version: 1,
+    title: 'Navigation Mission',
+    mainTreeId: 'MainTree',
+    nodes: [
+      {
+        id: 'root_sequence', type: 'behavior', position: { x: 475, y: 60 },
+        data: { label: 'Mission Sequence', nodeType: 'Sequence', category: 'control', registrationName: 'Sequence', ports: {}, notes: '主任务流程', breakpoint: false },
+      },
+      {
+        id: 'battery_check', type: 'behavior', position: { x: 170, y: 240 },
+        data: { label: 'Battery OK?', nodeType: 'Condition', category: 'condition', registrationName: 'BatteryOK', ports: { min_level: '{battery_threshold}' }, notes: '', breakpoint: false },
+      },
+      {
+        id: 'navigation_fallback', type: 'behavior', position: { x: 475, y: 240 },
+        data: { label: 'Navigate Safely', nodeType: 'Fallback', category: 'control', registrationName: 'Fallback', ports: {}, notes: '', breakpoint: false },
+      },
+      {
+        id: 'report_action', type: 'behavior', position: { x: 785, y: 240 },
+        data: { label: 'Report Result', nodeType: 'Action', category: 'action', registrationName: 'ReportResult', ports: { topic: '/mission/result' }, notes: '', breakpoint: false },
+      },
+      {
+        id: 'at_goal', type: 'behavior', position: { x: 340, y: 430 },
+        data: { label: 'Already At Goal?', nodeType: 'Condition', category: 'condition', registrationName: 'AtGoal', ports: { target: '{goal}' }, notes: '', breakpoint: false },
+      },
+      {
+        id: 'navigate_action', type: 'behavior', position: { x: 610, y: 430 },
+        data: { label: 'Navigate To Pose', nodeType: 'Action', category: 'action', registrationName: 'NavigateToPose', ports: { goal: '{goal}', server_timeout: '5000' }, notes: '调用 Nav2 action server', breakpoint: true },
+      },
+    ],
+    edges: [
+      { id: 'e-root-battery', source: 'root_sequence', target: 'battery_check', type: 'smoothstep' },
+      { id: 'e-root-nav', source: 'root_sequence', target: 'navigation_fallback', type: 'smoothstep' },
+      { id: 'e-root-report', source: 'root_sequence', target: 'report_action', type: 'smoothstep' },
+      { id: 'e-nav-goal', source: 'navigation_fallback', target: 'at_goal', type: 'smoothstep' },
+      { id: 'e-nav-action', source: 'navigation_fallback', target: 'navigate_action', type: 'smoothstep' },
+    ],
+  }
+}
+
+export function createEmptyDocument(): BehaviorTreeDocument {
+  return {
+    format: 'arborflow/project',
+    version: 1,
+    title: 'Untitled Tree',
+    mainTreeId: 'MainTree',
+    nodes: [],
+    edges: [],
+  }
+}
--- a/src/lib/layout.ts
+++ b/src/lib/layout.ts
@@ -0,0 +1,26 @@
+import dagre from '@dagrejs/dagre'
+import type { BehaviorTreeDocument } from '../types'
+
+const NODE_WIDTH = 210
+const NODE_HEIGHT = 84
+
+export function layoutDocument(document: BehaviorTreeDocument): BehaviorTreeDocument {
+  const graph = new dagre.graphlib.Graph()
+  graph.setDefaultEdgeLabel(() => ({}))
+  graph.setGraph({ rankdir: 'TB', nodesep: 46, ranksep: 88, marginx: 40, marginy: 40 })
+
+  document.nodes.forEach((node) => graph.setNode(node.id, { width: NODE_WIDTH, height: NODE_HEIGHT }))
+  document.edges.forEach((edge) => graph.setEdge(edge.source, edge.target))
+  dagre.layout(graph)
+
+  return {
+    ...document,
+    nodes: document.nodes.map((node) => {
+      const position = graph.node(node.id)
+      return {
+        ...node,
+        position: { x: position.x - NODE_WIDTH / 2, y: position.y - NODE_HEIGHT / 2 },
+      }
+    }),
+  }
+}
--- a/src/lib/project.ts
+++ b/src/lib/project.ts
@@ -0,0 +1,41 @@
+import type { BehaviorTreeDocument } from '../types'
+
+export function serializeProject(document: BehaviorTreeDocument) {
+  return JSON.stringify(document, null, 2) + '\n'
+}
+
+export function parseProject(content: string): BehaviorTreeDocument {
+  const value: unknown = JSON.parse(content)
+  if (!value || typeof value !== 'object') throw new Error('工程文件内容无效')
+  const candidate = value as Partial<BehaviorTreeDocument>
+  if (candidate.format !== 'arborflow/project' || candidate.version !== 1) {
+    throw new Error('不是受支持的 ArborFlow 工程文件')
+  }
+  if (!Array.isArray(candidate.nodes) || !Array.isArray(candidate.edges)) {
+    throw new Error('工程文件缺少节点或连接数据')
+  }
+  return candidate as BehaviorTreeDocument
+}
+
+export function downloadText(filename: string, content: string, type = 'text/plain') {
+  const blob = new Blob([content], { type })
+  const url = URL.createObjectURL(blob)
+  const anchor = document.createElement('a')
+  anchor.href = url
+  anchor.download = filename
+  anchor.click()
+  URL.revokeObjectURL(url)
+}
+
+export function pickBrowserFile(accept: string) {
+  return new Promise<{ path: string; content: string } | null>((resolve) => {
+    const input = document.createElement('input')
+    input.type = 'file'
+    input.accept = accept
+    input.onchange = async () => {
+      const file = input.files?.[0]
+      resolve(file ? { path: file.name, content: await file.text() } : null)
+    }
+    input.click()
+  })
+}
--- a/src/lib/runtime.ts
+++ b/src/lib/runtime.ts
@@ -0,0 +1,70 @@
+import type { BehaviorNode, RuntimeStatus } from '../types'
+
+const STATUS_BY_NUMBER: Record<number, RuntimeStatus> = {
+  0: 'IDLE',
+  1: 'RUNNING',
+  2: 'SUCCESS',
+  3: 'FAILURE',
+  4: 'SKIPPED',
+}
+
+export function normalizeStatus(value: unknown): RuntimeStatus | null {
+  if (typeof value === 'number') return STATUS_BY_NUMBER[value] || null
+  if (typeof value !== 'string') return null
+  const normalized = value.toUpperCase().trim() as RuntimeStatus
+  return ['IDLE', 'RUNNING', 'SUCCESS', 'FAILURE', 'SKIPPED'].includes(normalized) ? normalized : null
+}
+
+export function matchRuntimeNodes(reference: string, nodes: BehaviorNode[]) {
+  const cleanRef = reference.trim()
+  const pathTail = cleanRef.split('/').filter(Boolean).at(-1) || cleanRef
+  const exactId = nodes.filter((node) => node.id === cleanRef)
+  if (exactId.length) return exactId.map((node) => node.id)
+  const exactName = nodes.filter((node) => node.data.label === cleanRef || node.data.registrationName === cleanRef)
+  if (exactName.length) return exactName.map((node) => node.id)
+  return nodes
+    .filter((node) => node.data.label === pathTail || node.data.registrationName === pathTail)
+    .map((node) => node.id)
+}
+
+export interface ParsedMonitorUpdate {
+  reference: string
+  status: RuntimeStatus
+  timestamp?: number
+}
+
+export function parseMonitorMessage(raw: string): ParsedMonitorUpdate[] {
+  const message = JSON.parse(raw) as Record<string, unknown>
+  if (message.op === 'publish' && message.msg && typeof message.msg === 'object') {
+    const rosMessage = message.msg as Record<string, unknown>
+    if (typeof rosMessage.data === 'string') {
+      try {
+        return parseMonitorMessage(rosMessage.data)
+      } catch {
+        return []
+      }
+    }
+    return parseEvent(rosMessage)
+  }
+  if (message.type === 'snapshot' && message.nodes && typeof message.nodes === 'object') {
+    return Object.entries(message.nodes as Record<string, unknown>).flatMap(([reference, value]) => {
+      const statusValue = value && typeof value === 'object' ? (value as Record<string, unknown>).status : value
+      const status = normalizeStatus(statusValue)
+      return status ? [{ reference, status, timestamp: Number(message.timestamp) || undefined }] : []
+    })
+  }
+  if (Array.isArray(message.events)) {
+    return message.events.flatMap((event) => {
+      if (!event || typeof event !== 'object') return []
+      return parseEvent(event as Record<string, unknown>)
+    })
+  }
+  return parseEvent(message)
+}
+
+function parseEvent(message: Record<string, unknown>): ParsedMonitorUpdate[] {
+  const reference = message.nodeId ?? message.uid ?? message.node ?? message.name ?? message.path
+  const status = normalizeStatus(message.status)
+  if ((typeof reference !== 'string' && typeof reference !== 'number') || !status) return []
+  return [{ reference: String(reference), status, timestamp: Number(message.timestamp) || undefined }]
+}
--- a/src/lib/validation.ts
+++ b/src/lib/validation.ts
@@ -0,0 +1,50 @@
+import { maxChildren } from './catalog'
+import type { BehaviorTreeDocument, ValidationIssue } from '../types'
+
+export function validateDocument(document: BehaviorTreeDocument): ValidationIssue[] {
+  const issues: ValidationIssue[] = []
+  const incoming = new Map(document.nodes.map((node) => [node.id, 0]))
+  const outgoing = new Map(document.nodes.map((node) => [node.id, 0]))
+
+  document.edges.forEach((edge) => {
+    incoming.set(edge.target, (incoming.get(edge.target) || 0) + 1)
+    outgoing.set(edge.source, (outgoing.get(edge.source) || 0) + 1)
+  })
+
+  const roots = document.nodes.filter((node) => (incoming.get(node.id) || 0) === 0)
+  if (document.nodes.length === 0) issues.push({ severity: 'warning', message: '画布为空，请添加根节点' })
+  if (roots.length > 1) issues.push({ severity: 'error', message: `检测到 ${roots.length} 个根节点，导出 XML 前必须连接为一棵树` })
+  if (roots.length === 0 && document.nodes.length > 0) issues.push({ severity: 'error', message: '没有根节点，树中可能存在环路' })
+
+  document.nodes.forEach((node) => {
+    const childCount = outgoing.get(node.id) || 0
+    const limit = maxChildren(node.data.nodeType)
+    if (childCount > limit) {
+      issues.push({ severity: 'error', nodeId: node.id, message: `${node.data.label} 最多允许 ${limit} 个子节点` })
+    }
+    if (limit > 0 && childCount === 0) {
+      issues.push({ severity: 'warning', nodeId: node.id, message: `${node.data.label} 尚未连接子节点` })
+    }
+    if (!node.data.registrationName.trim()) {
+      issues.push({ severity: 'error', nodeId: node.id, message: `${node.data.label} 的注册名称不能为空` })
+    }
+  })
+
+  return issues
+}
+
+export function wouldCreateCycle(document: BehaviorTreeDocument, source: string, target: string) {
+  if (source === target) return true
+  const children = new Map<string, string[]>()
+  document.edges.forEach((edge) => children.set(edge.source, [...(children.get(edge.source) || []), edge.target]))
+  const stack = [target]
+  const visited = new Set<string>()
+  while (stack.length) {
+    const current = stack.pop()!
+    if (current === source) return true
+    if (visited.has(current)) continue
+    visited.add(current)
+    stack.push(...(children.get(current) || []))
+  }
+  return false
+}
--- a/src/lib/xml.ts
+++ b/src/lib/xml.ts
@@ -0,0 +1,114 @@
+import { createNode, NODE_DEFINITION_MAP } from './catalog'
+import { layoutDocument } from './layout'
+import type { BehaviorEdge, BehaviorNode, BehaviorTreeDocument, NodeType } from '../types'
+
+const CONTROL_AND_DECORATOR_TYPES = new Set<NodeType>([
+  'Sequence', 'Fallback', 'Parallel', 'ReactiveSequence', 'ReactiveFallback',
+  'Inverter', 'RetryUntilSuccessful', 'Repeat', 'ForceSuccess', 'ForceFailure',
+])
+
+function escapeXml(value: string) {
+  return value
+    .replaceAll('&', '&amp;')
+    .replaceAll('<', '&lt;')
+    .replaceAll('>', '&gt;')
+    .replaceAll('"', '&quot;')
+    .replaceAll("'", '&apos;')
+}
+
+function nodeTag(node: BehaviorNode) {
+  if (node.data.nodeType === 'SubTree') return 'SubTree'
+  if (node.data.nodeType === 'Action') return 'Action'
+  if (node.data.nodeType === 'Condition') return 'Condition'
+  return node.data.nodeType
+}
+
+export function documentToXml(document: BehaviorTreeDocument) {
+  if (document.nodes.length === 0) throw new Error('画布为空，无法导出 XML')
+  const incoming = new Map(document.nodes.map((node) => [node.id, 0]))
+  document.edges.forEach((edge) => incoming.set(edge.target, (incoming.get(edge.target) || 0) + 1))
+  const roots = document.nodes.filter((node) => (incoming.get(node.id) || 0) === 0)
+  if (roots.length !== 1) throw new Error(`XML 只能导出一棵完整树，当前检测到 ${roots.length} 个根节点`)
+
+  const byId = new Map(document.nodes.map((node) => [node.id, node]))
+  const children = new Map<string, BehaviorNode[]>()
+  document.edges.forEach((edge) => {
+    const child = byId.get(edge.target)
+    if (child) children.set(edge.source, [...(children.get(edge.source) || []), child])
+  })
+  children.forEach((items) => items.sort((a, b) => a.position.x - b.position.x))
+
+  const visited = new Set<string>()
+  const renderNode = (node: BehaviorNode, depth: number): string => {
+    if (visited.has(node.id)) throw new Error('树中存在重复引用或环路，无法导出 XML')
+    visited.add(node.id)
+    const indent = '    '.repeat(depth)
+    const tag = nodeTag(node)
+    const attributes: Array<[string, string]> = []
+    if (['Action', 'Condition', 'SubTree'].includes(tag)) attributes.push(['ID', node.data.registrationName])
+    if (node.data.label !== node.data.registrationName && node.data.label !== node.data.nodeType) attributes.push(['name', node.data.label])
+    Object.entries(node.data.ports).forEach(([key, value]) => {
+      if (key.trim()) attributes.push([key.trim(), value])
+    })
+    const attrs = attributes.map(([key, value]) => ` ${key}="${escapeXml(value)}"`).join('')
+    const childNodes = children.get(node.id) || []
+    if (childNodes.length === 0) return `${indent}<${tag}${attrs}/>`
+    const content = childNodes.map((child) => renderNode(child, depth + 1)).join('\n')
+    return `${indent}<${tag}${attrs}>\n${content}\n${indent}</${tag}>`
+  }
+
+  const treeXml = renderNode(roots[0], 2)
+  return `<?xml version="1.0" encoding="UTF-8"?>\n<root BTCPP_format="4" main_tree_to_execute="${escapeXml(document.mainTreeId)}">\n    <BehaviorTree ID="${escapeXml(document.mainTreeId)}">\n${treeXml}\n    </BehaviorTree>\n</root>\n`
+}
+
+function elementType(element: Element): NodeType {
+  const tag = element.tagName
+  if (NODE_DEFINITION_MAP.has(tag as NodeType)) return tag as NodeType
+  if (tag === 'SubTreePlus') return 'SubTree'
+  const children = Array.from(element.children)
+  return children.length ? 'Sequence' : 'Action'
+}
+
+export function xmlToDocument(xml: string): BehaviorTreeDocument {
+  const parsed = new DOMParser().parseFromString(xml, 'application/xml')
+  const parserError = parsed.querySelector('parsererror')
+  if (parserError) throw new Error(`XML 解析失败：${parserError.textContent?.split('\n')[0] || '格式错误'}`)
+  const behaviorTree = parsed.querySelector('BehaviorTree')
+  if (!behaviorTree) throw new Error('未找到 <BehaviorTree> 元素')
+  const treeRoot = Array.from(behaviorTree.children).find((child) => child.nodeType === Node.ELEMENT_NODE)
+  if (!treeRoot) throw new Error('BehaviorTree 中没有根节点')
+
+  const nodes: BehaviorNode[] = []
+  const edges: BehaviorEdge[] = []
+  let edgeIndex = 0
+
+  const visit = (element: Element, parentId?: string) => {
+    const type = elementType(element)
+    const node = createNode(type)
+    const idAttr = element.getAttribute('ID')
+    const registrationName = idAttr || (CONTROL_AND_DECORATOR_TYPES.has(type) ? type : element.tagName)
+    const label = element.getAttribute('name') || registrationName
+    const ports: Record<string, string> = {}
+    Array.from(element.attributes).forEach((attribute) => {
+      if (!['ID', 'name', '_autoremap'].includes(attribute.name)) ports[attribute.name] = attribute.value
+    })
+    node.data = { ...node.data, label, registrationName, ports }
+    nodes.push(node)
+    if (parentId) {
+      edges.push({ id: `edge_import_${edgeIndex++}`, source: parentId, target: node.id, type: 'smoothstep' })
+    }
+    Array.from(element.children).forEach((child) => visit(child, node.id))
+  }
+  visit(treeRoot)
+
+  const root = parsed.documentElement
+  const mainTreeId = root.getAttribute('main_tree_to_execute') || behaviorTree.getAttribute('ID') || 'MainTree'
+  return layoutDocument({
+    format: 'arborflow/project',
+    version: 1,
+    title: behaviorTree.getAttribute('ID') || 'Imported Tree',
+    mainTreeId,
+    nodes,
+    edges,
+  })
+}