diff --git a/src/graph.test.ts b/src/graph.test.ts index 32ce61a..31d745d 100644 --- a/src/graph.test.ts +++ b/src/graph.test.ts @@ -386,9 +386,9 @@ describe('ngraph', () => { // ^ // / // B - graph.addLink('a', 'c', creator.makeLock('ac')) - graph.addLink('b', 'c', creator.makeLock('bc')) - graph.addLink('c', 'd', creator.makeLock('cd')) + graph.addLink('a', 'c', creator.makeLinkLock('a', 'c')) + graph.addLink('b', 'c', creator.makeLinkLock('b', 'c')) + graph.addLink('c', 'd', creator.makeLinkLock('c', 'd')) const pathFinder = ngraphPath.aStar(graph, { oriented: true }) const s1Path = pathFinder.find('a', 'd').reverse() @@ -759,11 +759,11 @@ describe('ngraph', () => { s1LockNext.arrivedAt(s1Path.indexOf(nodeC)) expect(s1ForwardPath).toEqual([{index: 2, node: 'c'}, {index: 3, node: 'e'}]) - // agent2 obtains nodeD just before stepping off bidir lane - expect(s2ForwardPath).toEqual([{index: 0, node: 'd'}]) + expect(s2ForwardPath).toEqual([]) s1LockNext.arrivedAt(s1Path.indexOf(nodeE)) expect(s1ForwardPath).toEqual([{index: 3, node: 'e'}]) + // agent2 obtains nodeD and C after stepping off bidir lane expect(s2ForwardPath).toEqual([{index: 0, node: 'd'}, {index: 1, node: 'c'}]) }) @@ -1054,15 +1054,18 @@ describe('ngraph', () => { agent1at('e') expect(nextNodes1).toEqual([{index: 4, node: 'e'}, {index: 5, node: 'z'}]) - expect(nextNodes2).toEqual([{index: 0, node: 'g'}, {index: 1, node: 'f'}]) // seems a little early to obtain nodeF + expect(nextNodes2).toEqual([{index: 0, node: 'g'}]) + agent1at('z') + expect(nextNodes1).toEqual([{index: 5, node: 'z'}]) + expect(nextNodes2).toEqual([{index: 0, node: 'g'}, {index: 1, node: 'f'}]) // now we can move to F agent2at('f') - expect(nextNodes1).toEqual([{index: 4, node: 'e'}, {index: 5, node: 'z'}]) - expect(nextNodes2).toEqual([{index: 1, node: 'f'}]) // cant get E because agent1 is tehre + expect(nextNodes1).toEqual([{index: 5, node: 'z'}]) + expect(nextNodes2).toEqual([{index: 1, node: 'f'}, {index: 2, node: 'e'}]) - agent1at('z') + agent2at('e') expect(nextNodes1).toEqual([{index: 5, node: 'z'}]) - expect(nextNodes2).toEqual([{index: 1, node: 'f'}, {index: 2, node: 'e'}]) // now we can move to E + expect(nextNodes2).toEqual([{index: 2, node: 'e'}, {index: 3, node: 'd'}]) }) // TODO add test that shows we are waiting on distant edge //test('two robots opposing directions in a narrow corridor', () => { @@ -1153,6 +1156,73 @@ describe('ngraph', () => { lockNext.arrivedAt(i) } }) + test('agent encountered on bidir path with reversal', () => { + const graph = ngraphCreateGraph() + const creator = new Graferse>(node => node.id) + + const makeNode = (id: string) => graph.addNode(id, creator.makeLock(id)) + const nodeA = makeNode('a') + const nodeB = makeNode('b') + const nodeC = makeNode('c') + const nodeD = makeNode('d') + + // A ----> B <----> C + // | + // v + // D + + const lockAB = creator.makeLinkLock('a', 'b', false) + const lockBC = creator.makeLinkLock('b', 'c', true) + const lockCD = creator.makeLinkLock('b', 'd', false) + + const linkAB = graph.addLink('a', 'b', lockAB) + const linkBC = graph.addLink('b', 'c', lockBC) + const linkBD = graph.addLink('b', 'd', lockCD) + + const linkDB = graph.addLink('d', 'b', lockCD) + const linkCB = graph.addLink('c', 'b', lockBC) + const linkBA = graph.addLink('b', 'a', lockAB) + + const s1Path = [nodeA, nodeB, nodeC, nodeB, nodeD] + + const makeLocker = creator.makeMakeLocker(node => node.data, getLockForLink) + var s1ForwardPath: Array = [] + const s1LockNext = makeLocker("agent1").makePathLocker(s1Path)((nextNodes) => { s1ForwardPath = nextNodes }) + + //console.dir({nodeC}, {depth: null}) + expect(nodeC.data.requestLock('agent2', 'static')).toBeTruthy() + // all nodes are unlocked + expect(nodeA.data.isLocked()).toBeFalsy() + expect(nodeB.data.isLocked()).toBeFalsy() + expect(nodeC.data.isLocked()).toBeTruthy() // locked by static agent2 + expect(nodeD.data.isLocked()).toBeFalsy() + // all links are unlocked + expect(linkAB.data.isLocked()).toBeFalsy() + expect(linkBC.data.isLocked()).toBeFalsy() + expect(linkBD.data.isLocked()).toBeFalsy() + expect(linkDB.data.isLocked()).toBeFalsy() + expect(linkCB.data.isLocked()).toBeFalsy() + expect(linkBA.data.isLocked()).toBeFalsy() + + expect(s1ForwardPath).toEqual([]) + + s1LockNext.arrivedAt(s1Path.indexOf(nodeA)) + // its current and next nodes are locked + // nodeB omitted becuase agent encountered on bidir path + expect(s1ForwardPath).toEqual([{index: 0, node: 'a'}/*, {index: 1, node: 'b'}*/]) + expect(nodeA.data.isLocked()).toBeTruthy() + expect(nodeB.data.isLocked()).toBeFalsy() + expect(nodeC.data.isLocked()).toBeTruthy() // still locked by agent2 + expect(nodeD.data.isLocked()).toBeFalsy() + + // all links are locked until path ends + expect(linkAB.data.isLocked()).toBeFalsy() // its oneway, never locked + expect(linkBC.data.isLocked()).toBeFalsy() + expect(linkBD.data.isLocked()).toBeFalsy() + expect(linkDB.data.isLocked()).toBeFalsy() + expect(linkCB.data.isLocked()).toBeFalsy() + expect(linkBA.data.isLocked()).toBeFalsy() // its oneway, never locked + }) }) describe('Components', () => { @@ -1173,7 +1243,7 @@ describe('Components', () => { const linkLock = creator.makeLinkLock('up', 'down') // by default is directed edge expect(logSpyWarn).not.toHaveBeenCalled() expect(logSpyError).not.toHaveBeenCalled() - expect(linkLock.requestLock('test', 'up')).toEqual("FREE") + expect(linkLock.requestLock('test', 'up')).toBeTruthy() expect(logSpyWarn).toHaveBeenCalled() expect(logSpyError).toHaveBeenCalled() @@ -1186,29 +1256,33 @@ describe('Components', () => { test('single owner can lock both directions', () => { const creator = new Graferse(node => node.id) const linkLock = creator.makeLinkLock('up', 'down', true) // is bidirectional - expect(linkLock.requestLock('agent1', 'up')).toEqual("FREE") - expect(linkLock.requestLock('agent1', 'down')).toEqual("FREE") + expect(linkLock.requestLock('agent1', 'up')).toBeTruthy() + expect(linkLock.requestLock('agent1', 'down')).toBeTruthy() }) test('owner cannot lock both directions if multiple owners', () => { const creator = new Graferse(node => node.id) const linkLock = creator.makeLinkLock('up', 'down', true) // is bidirectional - expect(linkLock.requestLock('agent1', 'up')).toEqual("FREE") - expect(linkLock.requestLock('agent2', 'up')).toEqual("PRO") - expect(linkLock.requestLock('agent1', 'down')).toEqual("CON") + expect(linkLock.requestLock('agent1', 'up')).toBeTruthy() + expect(linkLock.requestLock('agent2', 'up')).toBeTruthy() + expect(linkLock.requestLock('agent1', 'down')).toBeFalsy() + expect(linkLock.isWaiting('agent1')).toBeTruthy() }) test('agent cannot lock if both directions already locked', () => { const creator = new Graferse(node => node.id) const linkLock = creator.makeLinkLock('up', 'down', true) // is bidirectional - expect(linkLock.requestLock('agent1', 'up')).toEqual("FREE") - expect(linkLock.requestLock('agent1', 'down')).toEqual("FREE") - expect(linkLock.requestLock('agent2', 'up')).toEqual("CON") - - linkLock.unlock('agent1', 'up') - expect(linkLock.requestLock('agent2', 'up')).toEqual("CON") - - expect(linkLock.requestLock('agent1', 'up')).toEqual("FREE") - linkLock.unlock('agent1', 'down') - expect(linkLock.requestLock('agent2', 'up')).toEqual("PRO") + expect(linkLock.requestLock('agent1', 'up')).toBeTruthy() + expect(linkLock.requestLock('agent1', 'down')).toBeTruthy() + expect(linkLock.requestLock('agent2', 'up')).toBeFalsy() + expect(linkLock.isWaiting('agent2')).toBeTruthy() + + expect(linkLock.unlock('agent1', 'up')).toEqual(new Set()) + expect(linkLock.requestLock('agent2', 'up')).toBeFalsy() + expect(linkLock.isWaiting('agent2')).toBeTruthy() + + expect(linkLock.requestLock('agent1', 'up')).toBeTruthy() + expect(linkLock.unlock('agent1', 'down')).toEqual(new Set(["agent2"])) + expect(linkLock.requestLock('agent2', 'up')).toBeTruthy() + expect(linkLock.isWaiting('agent2')).toBeFalsy() }) }) }) diff --git a/src/graph.ts b/src/graph.ts index 7ccba8d..e075f29 100644 --- a/src/graph.ts +++ b/src/graph.ts @@ -81,84 +81,111 @@ class Lock { } } -type LinkLockType = "FREE" | "PRO" | "CON" - class LinkLock { - private _lock: Lock = new Lock("linklock") - private _directions: Set = new Set() - private _allowed_directions: Set + private _lockers = new Map>() + private _waiters = new Map>() + private _otherdir = new Map() check (direction: string) { - if (!this._allowed_directions.has(direction)) - throw new Error(`no such direction ${direction}`) + if (!this._waiters.get(direction)) + throw new Error(`no such wait direction ${direction}`) + if (!this._lockers.get(direction)) + throw new Error(`no such lock direction ${direction}`) + if (!this._otherdir.get(direction)) + throw new Error(`no such other direction ${direction}`) } - constructor (from: string, to: string) { - this._allowed_directions = new Set([from, to]) + //isWaiting (who: string, direction: string) { + // check(direction) + // return this._waiters.get(direction).has(who) + //} + + isWaiting (who: string) { + return Array.from(this._otherdir.keys()).some(dir => { + const waiters = this._waiters.get(dir) + return waiters?.has(who) + }) } getDetails() { return { - directions: this._directions, - who: this._lock.lockedBy, + lockers: this._lockers, + waiters: this._waiters, } } - requestLock (byWhom: string, direction: string): LinkLockType { + constructor (to: string, from: string) { + this._lockers.set(to, new Set()) + this._lockers.set(from, new Set()) + + this._waiters.set(to, new Set()) + this._waiters.set(from, new Set()) + + this._otherdir.set(to, from) + this._otherdir.set(from, to) + } + + requestLock (byWhom: string, direction: string): boolean { this.check(direction) - // already locked by me - if (this._lock.isLocked(byWhom)) { - if (this._directions.size === 1 && !this._directions.has(direction)) { - if (this._lock.isLockedByOtherThan(byWhom)) - return "CON" - this._directions.add(direction) - } - return "FREE" - } - // if its locked by anyone else, in the direction we are going - if (this._lock.isLocked() && this._directions.size === 1 && this._directions.has(direction)) { - this._lock.forceLock(byWhom) // add ourselves to the list - return "PRO" - } + // I already have it locked in this direction + const lockers = this._lockers.get(direction) + if (!lockers) throw new Error("no lockers!") + if (lockers.has(byWhom)) + return true - // its not locked by anyone - if (this._lock.requestLock(byWhom, "link from " + direction)) { - this._directions.add(direction) - return "FREE" + // No one except me has it locked in the other direction + const against = this._lockers.get(this._otherdir.get(direction) as string) || new Set() + if (against.size === 0 || (against.size === 1 && against.has(byWhom))) { + lockers.add(byWhom) + return true } - return "CON" + debug(`Resource 'link from ${direction}' is locked, ${byWhom} should wait`) + this._waiters.get(direction)?.add(byWhom) + + return false } unlock (byWhom: string, direction?: string) { - if (direction) this.check(direction) - // if its locked only by a single robot - if (this._lock.isLocked(byWhom) && !this._lock.isLockedByOtherThan(byWhom)) { - if (direction) { - this._directions.delete(direction) - - // if we still are holding one direction, dont release lock - if (this._directions.size > 0) - return - } else { - this._directions.clear() - } - } + const dirsToUnlock = Array + .from(this._otherdir.keys()) + .filter(dir => !direction || dir === direction) + + dirsToUnlock.forEach(dir => this._lockers.get(dir)?.delete(byWhom)) + + const waiters = new Set() + + dirsToUnlock.forEach(dir => { + const otherdirwaiters = this._waiters.get(this._otherdir.get(dir) as string) + otherdirwaiters?.forEach(waiter => { + const tmp = new Set(this._lockers.get(dir)) + tmp.delete(waiter) + if (tmp.size === 0) { + waiters.add(waiter) + otherdirwaiters.delete(waiter) + } + }) + }) - return this._lock.unlock(byWhom) + return waiters } isLocked(byWhom?: string) { - return this._lock.isLocked(byWhom) + return Array.from(this._otherdir.keys()).some(dir => { + const lockers = this._lockers.get(dir) as Set + return byWhom + ? lockers.has(byWhom) + : lockers.size > 0 + }) } } class OnewayLinkLock extends LinkLock { - requestLock (byWhom: string, direction: string): LinkLockType { + requestLock (byWhom: string, direction: string): boolean { console.error("Who is trying to lock a non-bidir link?", {byWhom, direction}) console.warn("This will cause problems because it should stop locking here") - return "FREE" + return true } } @@ -233,20 +260,27 @@ class Graferse this.lockGroups.push(lockGroup) } - isLockGroupAvailable(lock: Lock, byWhom: string) { + getLockedGroupLock(lock: Lock, byWhom: string) { for(const lockGroup of this.lockGroups) { if (lockGroup.includes(lock)) { const lockedNode = lockGroup.filter(l => l !== lock) .find(l => l.isLockedByOtherThan(byWhom)) if (lockedNode) { - // wait on this locked node - if (lockedNode.requestLock(byWhom, "lockGroup")) { - throw new Error("lock was locked, but then not?") - } - return false + return lockedNode } } } + } + + isLockGroupAvailable(lock: Lock, byWhom: string) { + const lockedNode = this.getLockedGroupLock(lock, byWhom) + if (lockedNode) { + // wait on this locked node + if (lockedNode.requestLock(byWhom, "lockGroup")) { + throw new Error("lock was locked, but then not?") + } + return false + } return true } @@ -256,13 +290,44 @@ class Graferse ) { type NextNodes = (nextNodes: NextNode[], remaining: number) => void return (byWhom: string) => { + const waitOnObstructor = (destinationNode: T, encounteredLocks: Set) => { + const lock = getLock(destinationNode) + const lastEncouteredLock = lock.isLockedByOtherThan(byWhom) ? lock + : Array.from(encounteredLocks).at(-1) || this.getLockedGroupLock(lock, byWhom) + if (lastEncouteredLock) { + if (lastEncouteredLock.requestLock(byWhom, "capacity")) { + throw new Error("This lock should not succeed") + } + return true + } + } + const makePathLocker = (path: T[]) => (callback: NextNodes) => { // given an index in the path, tries to lock all bidirectional edges // till the last node in the path // returns false if first edge fails, otherwise returns true // as we can proceed some of the way in the same direction + + let pivotNode: T|undefined + const encounteredLocks = new Set() const tryLockAllBidirectionalEdges = (subpath: T[]) => { + // check if the path turns back on itself + if (subpath.length > 2) { + if (this.identity(subpath[0]) === this.identity(subpath[2])) + pivotNode = subpath[1] + } + if (subpath.length > 0) { + const lock = getLock(subpath[0]) + if (lock.isLockedByOtherThan(byWhom)) { + encounteredLocks.add(lock) + } + } if (subpath.length < 2) { + // we ended our path on a bidir edge (likely a trolly location) + // fail, and wait on the last lock we encountered + if (waitOnObstructor(pivotNode || subpath[0], encounteredLocks)) { + return false + } return true } // TODO will these locks and unlocks trigger waiters? @@ -272,13 +337,18 @@ class Graferse const fromNodeId = stringify(this.identity(subpath[0])) if (linkLock instanceof OnewayLinkLock) { console.debug(` ok - ${desc} not bidirectional`) + if (pivotNode) { + if (waitOnObstructor(pivotNode, encounteredLocks)) { + return false + } + } return true } const linkLockResult = linkLock.requestLock(byWhom, fromNodeId) // if it failed to lock because of opposing direction - if (linkLockResult === "CON") { + if (!linkLockResult) { console.warn(` fail - ${desc} locked against us`) console.warn(linkLock.getDetails()) return false @@ -356,11 +426,14 @@ class Graferse // TODO consider returning the length of obtained edge locks // if its > 0, even though further failed, allow the againt to retain the node lock // so we can enter corridors as far as we can and wait there + encounteredLocks.clear() + pivotNode = undefined if (!tryLockAllBidirectionalEdges(path.slice(i))) { // unlock previously obtained node lock whoCanMoveNow.addAll(lock.unlock(byWhom)) break } + console.log(`Encountered ${encounteredLocks.size} locks along the way`) nextNodes.push({node: this.identity(path[i]), index: i}) }