Robotics & Perception

-- Entities from sensor fusion
CREATE (p1:Entity {
  track_id: 'TRK-001',
  class: 'pedestrian',
  x: 5.2, y: 0.8, z: 0.0,
  vx: 0.3, vy: -0.1, vz: 0.0,
  _observation_class: 'observed',
  _confidence: 0.96
})
 
CREATE (p2:Entity {
  track_id: 'TRK-002',
  class: 'vehicle',
  x: 12.0, y: 3.1, z: 0.5,
  vx: 2.1, vy: 0.0, vz: 0.0,
  _observation_class: 'observed',
  _confidence: 0.91
})
 
-- Occlusion — predicted entity behind another
CREATE (p3:Entity {
  track_id: 'TRK-003',
  class: 'unknown',
  x: 14.5, y: 3.1, z: 0.0,
  _observation_class: 'predicted',
  _confidence: 0.38
})
 
-- Sensor provenance on every detection
CREATE (lidar:Sensor {id: 'LIDAR-FRONT', type: 'lidar', range_m: 100})
CREATE (lidar)-[:DETECTED {
  at: timestamp(),
  range_m: 5.8,
  intensity: 0.87,
  _confidence: 0.96
}]->(p1)

-- All entities within 20m (k-nearest, ArcFlow Spatial Index backed, >2000/s at 11K entities)
CALL algo.nearestNodes(point({x: 0.0, y: 0.0}), 'Entity', 50)
  YIELD node AS e, distance
WHERE distance < 20.0
RETURN e.track_id, e.class, e._observation_class, e._confidence, distance
ORDER BY distance
 
-- Trusted entities only (observed, high confidence) — safety-critical filter
CALL algo.nearestNodes(point({x: 0.0, y: 0.0}), 'Entity', 50)
  YIELD node AS e, distance
WHERE distance < 15.0
  AND e._observation_class = 'observed'
  AND e._confidence > 0.85
RETURN e.track_id, e.class, distance
 
-- Frustum: what is in the forward field of view?
CALL arcflow.scene.frustumQuery(0, 0, 0.5, 1, 0, 0, 60, 0.5, 50)
  YIELD node_id, label, x, y, z
 
-- Line-of-sight: is TRK-002 visible from current position?
CALL arcflow.scene.lineOfSight(ego_id, track_002_id)
  YIELD has_los, note

-- Where was entity TRK-001 at a recent checkpoint?
MATCH (e:Entity {track_id: 'TRK-001'}) AS OF seq 400
RETURN e.x, e.y, e.vx, e.vy
 
-- Trajectory over the last 2 seconds
MATCH (e:Entity {track_id: 'TRK-001'}) AS OF seq 500
RETURN e.x, e.y, e._confidence
 
-- Diagnose a near-miss: replay the world state at moment of incident
MATCH (e:Entity) AS OF seq 4821
RETURN e.track_id, e.class, e.x, e.y, e._observation_class, e._confidence

import { open } from 'arcflow'
 
const db = open('./data/robot-world-model')
 
// Hard stop: any high-confidence entity within 2m emergency radius
const safetyMonitor = db.subscribe(
  `CALL algo.nearestNodes(point({x: 0, y: 0}), 'Entity', 10)
   YIELD node AS e, distance
   WHERE distance < 2.0
     AND e._observation_class = 'observed'
     AND e._confidence > 0.8
   RETURN e.track_id, e.class, distance`,
  (event) => {
    if (event.added.length > 0) {
      triggerEmergencyStop()
    }
  }
)
 
// Slow zone: any entity entering a 5m caution radius
const cautionMonitor = db.subscribe(
  `CALL algo.nearestNodes(point({x: 0, y: 0}), 'Entity', 20)
   YIELD node AS e, distance
   WHERE distance < 5.0 AND distance >= 2.0
     AND e._observation_class IN ['observed', 'inferred']
   RETURN e.track_id, distance`,
  (event) => {
    if (event.added.length > 0) {
      reduceSpeed(event.added[0].get('distance'))
    }
  }
)

-- Find robots within communication range of each other
MATCH (a:Robot)-[:COMMS_WITH]->(b:Robot)
WHERE a.id <> b.id
RETURN a.id, b.id
 
-- Assign the nearest available robot to a task
MATCH (task:Task {status: 'unassigned'})
CALL algo.nearestNodes(point({x: task.x, y: task.y}), 'Robot', 5)
  YIELD node AS robot, distance
WHERE robot.status = 'idle'
WITH robot, task, distance
ORDER BY distance LIMIT 1
SET robot.status = 'assigned', task.status = 'assigned'
CREATE (robot)-[:EXECUTING]->(task)
RETURN robot.id, task.id, distance
 
-- Fleet-wide health: centrality-based criticality
CALL algo.pageRank()
YIELD nodeId, score
MATCH (r:Robot) WHERE id(r) = nodeId
RETURN r.id, r.status, r.battery_pct, score
ORDER BY score DESC

-- Promote a predicted track to observed when confidence exceeds threshold
MATCH (e:Entity {track_id: 'TRK-003'})
WHERE e._confidence >= 0.80
SET e._observation_class = 'observed'
 
-- Mark a track as lost (no detection in last 500ms)
MATCH (e:Entity)
WHERE NOT EXISTS {
  MATCH ()-[d:DETECTED]->(e)
  WHERE d.at > (timestamp() - 500)
}
SET e._observation_class = 'predicted', e._confidence = e._confidence * 0.85
 
-- Clean up tracks that have been lost too long
MATCH (e:Entity)
WHERE e._observation_class = 'predicted'
  AND e._confidence < 0.10
DETACH DELETE e

// Subscribe to ROS detection messages, write to world model
import { open } from 'arcflow'
import * as rclnodejs from 'rclnodejs'
 
const db = open('./data/robot-world-model')
 
rclnodejs.init().then(() => {
  const node = new rclnodejs.Node('arcflow_bridge')
  
  node.createSubscription(
    'vision_msgs/msg/Detection3DArray',
    '/detections',
    (msg) => {
      for (const det of msg.detections) {
        const { x, y, z } = det.bbox.center.position
        const confidence = det.results[0]?.score ?? 0.5
        const classId = det.results[0]?.hypothesis.class_id ?? 'unknown'
        
        db.mutate(`
          MERGE (e:Entity {track_id: $id})
          SET e.x = $x, e.y = $y, e.z = $z,
              e.class = $class,
              e._confidence = $conf,
              e._observation_class = 'observed'
        `, { id: det.id, x, y, z, class: classId, conf: confidence })
      }
    }
  )
  
  rclnodejs.spin(node)
})