tassello 0.1.0 → 0.2.0

package/dist/server/behavior_tree_playback.js

@@ -0,0 +1,188 @@
+// A rosbag of a behavior tree's log played back for one page, as the live log
+// would be (behavior_trees.ts): its transitions applied, on the bag's clock,
+// to a state of their own, sent as they change; at the page's speed, from
+// where it seeks. Read whole when opened: logs are small, and seeking back
+// replays them from the start.
+import { BT_RATE } from '../shared/protocol.js';
+import { readTree } from './behavior_tree_file.js';
+import { decodeLog, logLayout } from './behavior_tree_log.js';
+import { BehaviorTreeState } from './behavior_tree_state.js';
+import { bagFiles, readBag } from './rosbag.js';
+import { absoluteTopic, errorMessage } from './util.js';
+const TICK_MS = 50;
+// How often the page is told where the playback is, while playing
+const STATE_MS = 250;
+const MIN_SPEED = 0.05;
+const MAX_SPEED = 64;
+// Beyond them, the bag is too long to play whole
+const MAX_TRANSITIONS = 2_000_000;
+// The bag's log: of the topic if it has it, else its first
+export async function readLog(bag, topic) {
+    const logs = await readBag(await bagFiles(bag), (t) => /\/BehaviorTreeLog$/.test(t.type));
+    const found = (topic && logs.get(absoluteTopic(topic))) || [...logs.values()][0];
+    if (!found) {
+        throw new Error(`No behavior tree log in ${bag}: no nav2_msgs/msg/BehaviorTreeLog topic`);
+    }
+    const uids = logLayout(found.messages.map((message) => message.data));
+    if (uids === null) {
+        throw new Error(`No transition in ${bag}, of ${found.topic.name}`);
+    }
+    const messages = [];
+    let count = 0;
+    for (const { time, data } of found.messages) {
+        let transitions;
+        try {
+            transitions = decodeLog(data, uids);
+        }
+        catch {
+            continue;
+        }
+        count += transitions.length;
+        if (count > MAX_TRANSITIONS) {
+            throw new Error(`${bag} is too long to play: more than ${MAX_TRANSITIONS} transitions`);
+        }
+        messages.push({ time, transitions });
+    }
+    return { topic: found.topic.name, messages };
+}
+export class BehaviorTreePlayback {
+    watcher;
+    nodes = [];
+    messages = [];
+    state = new BehaviorTreeState([]);
+    // The next message to apply
+    next = 0;
+    topic = '';
+    start = 0;
+    end = 0;
+    time = 0;
+    playing = false;
+    speed = 1;
+    error = null;
+    timer = null;
+    lastTick = 0;
+    lastFlush = 0;
+    lastState = 0;
+    closed = false;
+    constructor(options, watcher) {
+        this.watcher = watcher;
+        void this.open(options);
+    }
+    // What the page asks: any of them
+    control({ playing, speed, time }) {
+        if (this.messages.length === 0) {
+            return;
+        }
+        if (speed !== undefined && Number.isFinite(speed)) {
+            this.speed = Math.min(MAX_SPEED, Math.max(MIN_SPEED, speed));
+        }
+        if (time !== undefined && Number.isFinite(time)) {
+            this.seek(time);
+        }
+        if (playing !== undefined) {
+            // From the start again, once at the end
+            if (playing && this.time >= this.end) {
+                this.seek(this.start);
+            }
+            this.playing = playing;
+            if (playing && !this.timer) {
+                this.lastTick = Date.now();
+                this.timer = setInterval(() => this.tick(), TICK_MS);
+            }
+            else if (!playing) {
+                this.stopTimer();
+            }
+        }
+        this.sendState();
+    }
+    close() {
+        this.closed = true;
+        this.stopTimer();
+    }
+    async open({ bag, topic, tree: source, uploadsDir, logger }) {
+        this.watcher.onStatus('waiting', `Reading ${bag}…`);
+        const [{ tree }, log] = await Promise.all([
+            readTree(source, uploadsDir, logger),
+            readLog(bag, topic).catch((e) => new Error(errorMessage(e))),
+        ]);
+        if (this.closed) {
+            return;
+        }
+        this.nodes = tree.nodes;
+        this.state = new BehaviorTreeState(this.nodes);
+        this.watcher.onTree(tree);
+        if (log instanceof Error) {
+            this.error = log.message;
+            this.watcher.onStatus('error', log.message);
+            this.watcher.onUpdate({ reset: true, ...this.state.snapshot() });
+            this.sendState();
+            return;
+        }
+        this.topic = log.topic;
+        this.messages = log.messages;
+        this.start = this.messages[0]?.time ?? 0;
+        this.end = this.messages.at(-1)?.time ?? 0;
+        this.time = this.start;
+        this.watcher.onStatus('streaming', `Playing ${bag}`);
+        this.seek(this.start);
+        this.control({ playing: true });
+    }
+    tick() {
+        const now = Date.now();
+        this.time = Math.min(this.end, this.time + (now - this.lastTick) * this.speed);
+        this.lastTick = now;
+        this.advance();
+        if (this.time >= this.end) {
+            this.playing = false;
+            this.stopTimer();
+            this.flush();
+            this.sendState();
+            return;
+        }
+        if (now - this.lastFlush >= 1000 / BT_RATE) {
+            this.flush();
+        }
+        if (now - this.lastState >= STATE_MS) {
+            this.sendState();
+        }
+    }
+    // The messages recorded until the playback's time
+    advance() {
+        while (this.next < this.messages.length && this.messages[this.next].time <= this.time) {
+            const { time, transitions } = this.messages[this.next++];
+            for (const transition of transitions) {
+                this.state.apply(transition, time);
+            }
+        }
+    }
+    // Back: the state again from the start. The page gets it whole.
+    seek(time) {
+        const target = Math.min(this.end, Math.max(this.start, time));
+        if (target < this.time) {
+            this.state = new BehaviorTreeState(this.nodes);
+            this.next = 0;
+        }
+        this.time = target;
+        this.advance();
+        this.watcher.onUpdate({ reset: true, ...this.state.snapshot() });
+        this.lastFlush = Date.now();
+    }
+    flush() {
+        this.lastFlush = Date.now();
+        const changes = this.state.takeChanges();
+        if (changes) {
+            this.watcher.onUpdate({ reset: false, ...changes });
+        }
+    }
+    sendState() {
+        this.lastState = Date.now();
+        const { topic, start, end, time, playing, speed, error } = this;
+        this.watcher.onPlayback({ topic, start, end, time, playing, speed, error });
+    }
+    stopTimer() {
+        if (this.timer) {
+            clearInterval(this.timer);
+            this.timer = null;
+        }
+    }
+}

package/dist/server/behavior_tree_state.d.ts

@@ -0,0 +1,38 @@
+import type { BtNode, BtNodeState, BtTransition } from '../shared/protocol.ts';
+import type { LogTransition } from './behavior_tree_log.ts';
+export interface Changes {
+    added: BtNode[];
+    states: BtNodeState[];
+    transitions: BtTransition[];
+    mismatch: string | null;
+}
+export declare class BehaviorTreeState {
+    readonly nodes: BtNode[];
+    private readonly treeSize;
+    private readonly states;
+    private readonly depths;
+    private readonly byName;
+    private readonly added;
+    private uidOffset;
+    private uidOffsets;
+    private uidSamples;
+    private uidMisses;
+    private uidUnfit;
+    private seq;
+    private readonly recent;
+    private readonly dirty;
+    private pending;
+    private addedSince;
+    private mismatchSent;
+    constructor(tree: BtNode[]);
+    apply(transition: LogTransition, now: number): void;
+    takeChanges(): Changes | null;
+    snapshot(): Changes;
+    private blank;
+    private stateOf;
+    private resolve;
+    private byUid;
+    private byStatus;
+    private add;
+    private mismatch;
+}

package/dist/server/behavior_tree_state.js

@@ -0,0 +1,220 @@
+// What a behavior tree's nodes are doing, from the transitions of its log:
+// which node each transition is of, then the node's status, last result and
+// counts, and the latest transitions.
+//
+// The log names the nodes, and since Nav2 Jazzy gives their UIDs too, which
+// BT.CPP numbers in the order of the tree's nodes (see behavior_tree_file.ts):
+// the offset between the two is found from the first transitions, after which
+// each UID is its node. Without UIDs, or when they do not fit the tree, nodes
+// are found by name; between nodes of the same name (a RecoveryNode named
+// after its action, the many Sequences), by what the transition says of the
+// node's status and what is known of its parent's: BT.CPP only ticks, halts
+// and resets the children of a running node.
+// Kept for the pages to come, and between two updates
+const MAX_TRANSITIONS = 300;
+// Of the log not in the tree: beyond them, the log is not worth listing
+const MAX_ADDED = 1000;
+// UIDs that do not fit in a row: the robot runs another tree, look again
+const UID_MISSES = 3;
+export class BehaviorTreeState {
+    nodes;
+    treeSize;
+    states = [];
+    depths = [];
+    byName = new Map();
+    // The nodes added, by UID and name, or name
+    added = new Map();
+    uidOffset = null;
+    // The offsets the transitions so far allow, until one is left, and how
+    // many transitions agreed on them
+    uidOffsets = null;
+    uidSamples = 0;
+    uidMisses = 0;
+    uidUnfit = false;
+    seq = 0;
+    recent = [];
+    // Since the last takeChanges
+    dirty = new Set();
+    pending = [];
+    addedSince;
+    mismatchSent = null;
+    constructor(tree) {
+        this.nodes = [...tree];
+        this.treeSize = tree.length;
+        this.addedSince = tree.length;
+        tree.forEach((node, index) => {
+            this.states.push(this.blank(index));
+            this.depths.push(node.parent < 0 ? 0 : this.depths[node.parent] + 1);
+            const same = this.byName.get(node.name);
+            if (same) {
+                same.push(index);
+            }
+            else {
+                this.byName.set(node.name, [index]);
+            }
+        });
+    }
+    apply(transition, now) {
+        const index = this.resolve(transition);
+        if (index === null) {
+            return;
+        }
+        const { from, to } = transition;
+        const state = this.states[index];
+        state.status = to;
+        state.since = now;
+        state.changes++;
+        state.seq = ++this.seq;
+        if (to === 'SUCCESS' || to === 'FAILURE') {
+            state.result = to;
+            state[to === 'SUCCESS' ? 'successes' : 'failures']++;
+        }
+        this.dirty.add(index);
+        // Ticked, or running: so are its ancestors, whose start came before
+        if ((to !== 'IDLE' && to !== 'SKIPPED') || from === 'RUNNING') {
+            for (let p = this.nodes[index].parent; p >= 0 && this.states[p].status === null; p = this.nodes[p].parent) {
+                this.states[p].status = 'RUNNING';
+                this.dirty.add(p);
+            }
+        }
+        const entry = { time: transition.time, node: index, from, to };
+        this.recent.push(entry);
+        if (this.recent.length > MAX_TRANSITIONS) {
+            this.recent.shift();
+        }
+        this.pending.push(entry);
+        if (this.pending.length > MAX_TRANSITIONS) {
+            this.pending.shift();
+        }
+    }
+    // What changed since the last call, null if nothing
+    takeChanges() {
+        const mismatch = this.mismatch();
+        if (this.dirty.size === 0 && this.pending.length === 0 && mismatch === this.mismatchSent) {
+            return null;
+        }
+        const changes = {
+            added: this.nodes.slice(this.addedSince),
+            states: [...this.dirty].map((index) => this.stateOf(index)),
+            transitions: this.pending,
+            mismatch,
+        };
+        this.dirty.clear();
+        this.pending = [];
+        this.addedSince = this.nodes.length;
+        this.mismatchSent = mismatch;
+        return changes;
+    }
+    // Everything known, for a page that comes; what changed so far is sent
+    // with it, not again
+    snapshot() {
+        this.takeChanges();
+        return {
+            added: this.nodes.slice(this.treeSize),
+            states: this.states.flatMap((state, index) => (state.status === null ? [] : [this.stateOf(index)])),
+            transitions: [...this.recent],
+            mismatch: this.mismatchSent,
+        };
+    }
+    blank(node) {
+        return { node, status: null, result: null, since: 0, changes: 0, successes: 0, failures: 0, seq: 0 };
+    }
+    stateOf(index) {
+        const { seq: _, ...state } = this.states[index];
+        return state;
+    }
+    resolve(transition) {
+        if (this.treeSize > 0 && this.byName.has(transition.name)) {
+            return (transition.uid === null ? null : this.byUid(transition.uid, transition.name)) ?? this.byStatus(transition);
+        }
+        return this.add(transition);
+    }
+    byUid(uid, name) {
+        if (this.uidOffset !== null) {
+            const index = uid - this.uidOffset;
+            if (index >= 0 && index < this.treeSize && this.nodes[index].name === name) {
+                this.uidMisses = 0;
+                return index;
+            }
+            if (++this.uidMisses >= UID_MISSES) {
+                this.uidOffset = null;
+                this.uidMisses = 0;
+                this.uidUnfit = true;
+            }
+            return null;
+        }
+        const offsets = new Set(this.byName.get(name).map((index) => uid - index));
+        this.uidOffsets = this.uidOffsets ? new Set([...this.uidOffsets].filter((o) => offsets.has(o))) : offsets;
+        this.uidSamples++;
+        if (this.uidOffsets.size === 1 && this.uidSamples >= 2) {
+            this.uidOffset = [...this.uidOffsets][0];
+            this.uidOffsets = null;
+            this.uidSamples = 0;
+            this.uidUnfit = false;
+            return uid - this.uidOffset;
+        }
+        if (this.uidOffsets.size === 0) {
+            // Not the file's order: by name, and again from the next transitions
+            this.uidOffsets = null;
+            this.uidSamples = 0;
+            this.uidUnfit = true;
+        }
+        return null;
+    }
+    // The node of that name the transition fits best: in the status it leaves,
+    // under a running parent (or one not known yet, which may be). Ties go to
+    // the node whose parent changed last; then parents start before their
+    // children, which finish and are reset first.
+    byStatus({ name, from, to }) {
+        const candidates = this.byName.get(name);
+        if (candidates.length === 1) {
+            return candidates[0];
+        }
+        const starting = from === 'IDLE';
+        let best = candidates[0];
+        let bestKey = [];
+        for (const index of candidates) {
+            const status = this.states[index].status;
+            const parent = this.nodes[index].parent;
+            const parentStatus = parent < 0 ? 'RUNNING' : this.states[parent].status;
+            const key = [
+                (status === from ? 4 : status === null ? 2 : 0) + (parentStatus === 'RUNNING' || parentStatus === null ? 2 : 0),
+                parent < 0 ? 0 : this.states[parent].seq,
+                starting ? -this.depths[index] : this.depths[index],
+            ];
+            const better = key.findIndex((value, i) => value !== bestKey[i]);
+            if (bestKey.length === 0 || (better !== -1 && key[better] > bestKey[better])) {
+                best = index;
+                bestKey = key;
+            }
+        }
+        return best;
+    }
+    // A node not in the tree: listed after the others
+    add({ uid, name }) {
+        const key = uid === null ? name : `${uid} ${name}`;
+        let index = this.added.get(key);
+        if (index === undefined) {
+            if (this.added.size >= MAX_ADDED) {
+                return null;
+            }
+            index = this.nodes.length;
+            this.nodes.push({ name, type: name, kind: null, parent: -1, ports: {} });
+            this.states.push(this.blank(index));
+            this.depths.push(0);
+            this.added.set(key, index);
+        }
+        return index;
+    }
+    mismatch() {
+        const missing = this.nodes.length - this.treeSize;
+        if (this.treeSize === 0) {
+            return null;
+        }
+        if (missing > 0) {
+            const names = this.nodes.slice(this.treeSize, this.treeSize + 3).map((node) => node.name).join(', ');
+            return `${missing === 1 ? 'A node' : `${missing} nodes`} of the log not in the tree file (${names}${missing > 3 ? ', …' : ''}): is it the tree the robot runs?`;
+        }
+        return this.uidUnfit ? 'UIDs of the log that do not fit the tree file: its nodes are found by name' : null;
+    }
+}

package/dist/server/behavior_trees.d.ts

@@ -0,0 +1,29 @@
+import type rclnodejs from 'rclnodejs';
+import { type BehaviorTreeRecording, type BehaviorTreeUpdate } from '../shared/protocol.ts';
+import { type TreeInfo } from './behavior_tree_file.ts';
+import { BehaviorTreePlayback, type PlaybackWatcher } from './behavior_tree_playback.ts';
+import type { StreamState } from './frame_stream.ts';
+import { type Logger } from './util.ts';
+export type RecordingInfo = BehaviorTreeRecording['recording'];
+export interface BehaviorTreeWatcher {
+    onStatus: (state: StreamState, message: string) => void;
+    onTree: (tree: TreeInfo) => void;
+    onUpdate: (update: Omit<BehaviorTreeUpdate, 'type' | 'id'>) => void;
+    onRecording: (recording: RecordingInfo) => void;
+}
+export declare class BehaviorTrees {
+    private readonly node;
+    private readonly logger;
+    private readonly uploadsDir;
+    private readonly topics;
+    private readonly monitors;
+    private readonly recordings;
+    constructor(node: rclnodejs.Node, logger: Logger, uploadsDir: string);
+    watch(topic: string, tree: string, watcher: BehaviorTreeWatcher): () => void;
+    record(topic: string, path: string | null): Promise<string | null>;
+    play(bag: string, topic: string, tree: string, watcher: PlaybackWatcher): BehaviorTreePlayback;
+    stopAll(): Promise<void>;
+    private recordingOf;
+    private announce;
+    private forget;
+}