488 lines
14 KiB
JavaScript
488 lines
14 KiB
JavaScript
// const CircQueue = require('./circQueue');
|
|
// const TryLock = require('./tryLock');
|
|
// import TryLock from './tryLock.js';
|
|
// import CircQueue from './circQueue.js';
|
|
|
|
|
|
/* Symbol 防止值重复 */
|
|
/**
|
|
* 模式枚举
|
|
* 导航模式: 绘制规划已完成路径和当前路径
|
|
* 定位模式: 绘制当前点
|
|
* 轨迹模式: 实时绘制历史轨迹
|
|
*/
|
|
const TPMode = Object.freeze({
|
|
NAVIGATION: Symbol('NAVIGATION'),
|
|
LOCATION: Symbol('LOCATION'),
|
|
TRACK: Symbol('TRACK'),
|
|
});
|
|
|
|
/**
|
|
* 动作枚举
|
|
* 用于导航模式下为 upsert 提供策略
|
|
* UPDATE : 更新
|
|
* ADD : 添加
|
|
*/
|
|
const TPAction = Object.freeze({
|
|
UPDATE: Symbol('UPDATE'),
|
|
ADD: Symbol('ADD'),
|
|
});
|
|
|
|
/**
|
|
* 仅测试用
|
|
* linux 运行 TEST_LOCK=true node testTrace.js
|
|
* windows 运行 $env:TEST_LOCK="true"; node .\testTrace.js
|
|
*/
|
|
// const TEST_LOCK = process.env.TEST_LOCK === 'true';
|
|
|
|
/*==============================================================================================================
|
|
*
|
|
* tracePoint 类
|
|
*
|
|
* =============================================================================================================
|
|
* 轨迹绘制的方式可以分为以下模式:
|
|
* 导航模式:
|
|
* 1. 绘制规划已完成路径和当前路径
|
|
* 2. 添加完成点时调用: upsert(point, TPAction.ADD)
|
|
* 3. 更新当前点时调用: upsert(point)
|
|
* 4. 轨迹点数组: [已完成点1, 已完成点2, ..., 已完成点N-1, 新完成点, 当前点]
|
|
* 示例: 轨迹点: | * | * | * | * | * | * |
|
|
* ↑
|
|
* 当前点
|
|
* 5. 说明:
|
|
* * 轨迹点数组中存放的是已完成的点 + 当前点
|
|
* * 其中当前点永远位于数组的末尾
|
|
* -------------------------------------------------------------------------------------------
|
|
* 定位模式:
|
|
* 1. 绘制当前点, 轨迹点数组: [当前点]
|
|
* 2. 调用方法: upsert(point)
|
|
* -------------------------------------------------------------------------------------------
|
|
* 轨迹模式:
|
|
* 1. 实时绘制历史轨迹, 轨迹点数组: [实时点1, 实时点2, ..., 实时点N]
|
|
* 2. 调用方法: upsert(point)
|
|
* -------------------------------------------------------------------------------------------
|
|
* 注意:
|
|
* 1. 复位调用 reset
|
|
* 2. 工作模式初始化默认为 TPMode.LOCATION 定位模式, 更换模式调用 setMode
|
|
* 3. 获取轨迹点调用 getTracePoint 方法
|
|
* 4. 添加或更新点调用 upsert
|
|
*
|
|
*=============================================================================================================*/
|
|
class TracePoint
|
|
{
|
|
/* 成员变量(私有) */
|
|
#queue; /* 循环队列, 用于存放等待处理的点 */
|
|
#tracePoint; /* 轨迹点数组, 存放已完成点 + 当前点 */
|
|
#completePointIndex; /* 完成点索引, 指向当前点之前的最后一个要插入的完成点的位置 */
|
|
#lock; /* 锁, 用于防止多线程同时更新 */
|
|
#mode; /* 模式管理器 */
|
|
|
|
/* ========================
|
|
* TEST ONLY: BEGIN
|
|
* ========================*/
|
|
tryLock()
|
|
{
|
|
return this.#lock.tryLock();
|
|
}
|
|
release()
|
|
{
|
|
this.#lock.release();
|
|
}
|
|
/* ========================
|
|
* TEST ONLY: END
|
|
* ========================*/
|
|
|
|
/* 构造函数 */
|
|
constructor()
|
|
{
|
|
this.#queue = new CircQueue(5, {
|
|
deepCopy: true,
|
|
maxMemoryBytes: 1024*1024, /* 1MB 限制 */
|
|
});
|
|
|
|
|
|
this.#tracePoint = []; /* 存放轨迹, 已完成点 + 当前点 */
|
|
this.#completePointIndex = 0; /* 完成点索引 */
|
|
this.#lock = new TryLock(); /* 锁, 用于防止多线程同时更新 */
|
|
this.#mode = TPMode.LOCATION; /* 默认为定位模式 */
|
|
}
|
|
|
|
/* 复位 */
|
|
reset()
|
|
{
|
|
/* 重置轨迹点 */
|
|
let len = this.#queue.getDepth();
|
|
this.#queue.discard(len); /* 清空队列 */
|
|
this.#tracePoint = []; /* 清空轨迹点数组 */
|
|
this.#completePointIndex = 0; /* 重置完成点索引 */
|
|
this.#lock.release(); /* 释放锁 */
|
|
}
|
|
|
|
/**
|
|
* mode: 模式选择器, 见枚举 TPMode
|
|
* 说明:
|
|
* 导航模式: 绘制规划已完成路径和当前路径
|
|
* 定位模式: 绘制当前点
|
|
* 轨迹模式: 实时绘制历史轨迹
|
|
*/
|
|
setMode(mode)
|
|
{
|
|
this.reset();
|
|
this.#mode = mode;
|
|
}
|
|
|
|
/**
|
|
* point : 需要添加或更细的点
|
|
* act : 仅导航模式下有效, 缺省为 TPAction.UPDATE , 具体如下:
|
|
* 1. 添加完成点时调用: upsert(point, TPAction.ADD)
|
|
* 2. 更新当前点时调用: upsert(point)
|
|
*/
|
|
upsert(point, act = TPAction.UPDATE)
|
|
{
|
|
if(this.#mode === TPMode.NAVIGATION)
|
|
{
|
|
(act === TPAction.UPDATE)
|
|
? this.#updateCurrentPoint(point)
|
|
: this.#addCompletePoint(point);
|
|
}
|
|
else if(this.#mode === TPMode.LOCATION)
|
|
{
|
|
this.#updateCurrentPoint(point);
|
|
}
|
|
else if(this.#mode === TPMode.TRACK)
|
|
{
|
|
this.#addCompletePoint(point);
|
|
}
|
|
}
|
|
|
|
/* 获取轨迹 */
|
|
getTracePoint()
|
|
{
|
|
/* 尝试锁定, 防止多线程同时更新 */
|
|
if( this.#lock.tryLock() )
|
|
{
|
|
const trace = this.#tracePoint.slice(); /* 深拷贝, 避免外部修改导致内部出错 */
|
|
this.#lock.release(); /* 解锁 */
|
|
return trace;
|
|
}
|
|
else
|
|
{
|
|
console.warn('getTracePoint: lock!!!');
|
|
return null; /* 锁定中, 返回空 */
|
|
}
|
|
}
|
|
|
|
/* 更新当前点 */
|
|
#updateCurrentPoint(point)
|
|
{
|
|
if (!point)
|
|
{
|
|
console.error('updateCurrentPoint: point is NULL');
|
|
return;
|
|
}
|
|
|
|
/* 尝试锁定, 防止多线程同时更新 */
|
|
if( this.#lock.tryLock() )
|
|
{
|
|
/**
|
|
* 导航模式: 当前点只插在尾部,若轨迹数组尚未插入完成点, 则丢弃
|
|
* 定位模式: 轨迹数组只有一个点
|
|
*/
|
|
if(this.#mode === TPMode.NAVIGATION)
|
|
{
|
|
if (this.#tracePoint.length === 0)
|
|
{
|
|
console.log('updateCurrentPoint: tracePoint is empty , wait first completed point, discard current point:', point);
|
|
}
|
|
else
|
|
{
|
|
/* 只保留完成点部分, 再插入新的当前点 */
|
|
this.#tracePoint = this.#tracePoint.slice(0, this.#completePointIndex);
|
|
this.#tracePoint.push(point);
|
|
|
|
/* 触发事件 newCurrentPoint, 驱动外部更新 */
|
|
// emit
|
|
}
|
|
}
|
|
else if(this.#mode === TPMode.LOCATION)
|
|
{
|
|
this.#tracePoint[0] = point; /* JS 支持自动扩展数组 */
|
|
}
|
|
|
|
this.#lock.release(); /* 解锁 */
|
|
}
|
|
/* 如果轨迹数组正在操作, 先存放至队列中 */
|
|
else
|
|
{
|
|
console.warn('updateCurrentPoint: lock!!!, discard current point:', point);
|
|
}
|
|
}
|
|
|
|
|
|
/* 添加完成点 */
|
|
#addCompletePoint(point)
|
|
{
|
|
if (!point)
|
|
{
|
|
console.error('addCompletePoint: point is NULL');
|
|
return;
|
|
}
|
|
|
|
/* 尝试锁定, 防止多线程同时更新 */
|
|
if( this.#lock.tryLock() )
|
|
{
|
|
try
|
|
{
|
|
/* 先处理队列中积压的点 */
|
|
while (this.#queue.isEmpty() === false)
|
|
{
|
|
/* 如果队列不为空, 取出队列中的点 */
|
|
const point = this.#queue.out();
|
|
this.#addPoint(point);
|
|
}
|
|
|
|
/* 添加完成点 */
|
|
this.#addPoint(point);
|
|
|
|
/* 触发事件 newCompletePoint, 驱动外部更新 */
|
|
// emit
|
|
}
|
|
catch (error)
|
|
{
|
|
/* 错误处理, js 可以输出到 .err 文件？ */
|
|
|
|
/* 打印 */
|
|
console.error('addCompletePoint failure:', error);
|
|
}
|
|
finally
|
|
{
|
|
this.#lock.release(); /* 无论成功失败都解锁, 避免无法使用 */
|
|
}
|
|
}
|
|
/* 如果轨迹数组正在操作, 先存放至队列中 */
|
|
else
|
|
{
|
|
this.#queue.enter(point);
|
|
console.warn('addCompletePoint: lock!!!, enter queue, wait disposing:', point);
|
|
return;
|
|
}
|
|
}
|
|
|
|
#addPoint(point)
|
|
{
|
|
if (!point)
|
|
{
|
|
console.error('point NULL:');
|
|
return;
|
|
}
|
|
|
|
/* 若当前点存在，说明当前数组末尾已插入一个当前点, 截断轨迹数组到当前点，将新完成点插入在当前点之前 */
|
|
const hasCurrentPoint = (this.#tracePoint.length > 0 && this.#completePointIndex < this.#tracePoint.length);
|
|
|
|
if (hasCurrentPoint)
|
|
{
|
|
/* 截断避免多添加了无效点, 然后再当前点之前插入完成点 */
|
|
this.#tracePoint = this.#tracePoint.slice(0, this.#completePointIndex);
|
|
this.#tracePoint.splice(this.#completePointIndex, 0, point);
|
|
}
|
|
/* 首次添加轨迹点需要先尾插 */
|
|
else
|
|
{
|
|
this.#tracePoint.push(point);
|
|
}
|
|
|
|
/* 更新当前点索引 */
|
|
this.#completePointIndex ++;
|
|
}
|
|
}
|
|
|
|
class CircQueue
|
|
{
|
|
/**
|
|
* 构造函数
|
|
* capacity ：队列可容纳的最大项目数
|
|
* options ：可选的配置对象，用于自定义环形队列的行为。它支持以下配置项：
|
|
* deepCopy ：控制入队时是否深拷贝元素, 默认 true
|
|
* maxMemoryBytes ：队列的最大内存限制 (单位：字节)，默认 Infinity, 不限制内存
|
|
* 若传 1024 * 1024 代表限制 1 M ,超出则队满, 队列会拒绝入队
|
|
*/
|
|
constructor(capacity, options = {})
|
|
{
|
|
this.capacity = capacity; /* 队列容量 (最多存储项目数) */
|
|
this.buffer = new Array(capacity); /* 队列存储区，使用数组存任意对象 */
|
|
this.head = 0; /* 队头索引 */
|
|
this.tail = 0; /* 队尾索引 */
|
|
this.isFull = false; /* 队满标志 */
|
|
|
|
this.deepCopy = options.deepCopy ?? true; /* 是否启用深拷贝 (默认启用) */
|
|
this.maxMemoryBytes = options.maxMemoryBytes ?? Infinity; /* 队列最大内存限制 (单位：字节) */
|
|
this.currentMemoryBytes = 0; /* 当前已使用内存大小 (单位：字节) */
|
|
}
|
|
|
|
/* 判断队列是否为空 */
|
|
isEmpty()
|
|
{
|
|
return this.head === this.tail && !this.isFull;
|
|
}
|
|
|
|
/* 判断队列是否已满 */
|
|
isFullFn()
|
|
{
|
|
return this.isFull;
|
|
}
|
|
|
|
/* 估算对象大小 (以 JSON 字符串的 UTF-8 编码长度作为近似值) */
|
|
_estimateSize(obj)
|
|
{
|
|
try
|
|
{
|
|
return Buffer.byteLength(JSON.stringify(obj), 'utf8'); /* Node.js 中用于计算字节长度 */
|
|
}
|
|
catch
|
|
{
|
|
return 0; /* 若对象不可序列化，返回 0 (避免出错) */
|
|
}
|
|
}
|
|
|
|
/* 深拷贝对象 (基于 JSON 的方式，仅支持可序列化对象) */
|
|
_deepClone(obj)
|
|
{
|
|
return this.deepCopy ? JSON.parse(JSON.stringify(obj)) : obj;
|
|
}
|
|
|
|
/* 入队操作：将对象放入队尾 */
|
|
enter(item)
|
|
{
|
|
const itemSize = this._estimateSize(item); /* 估算项目内存大小 */
|
|
if (this.isFullFn() || (this.currentMemoryBytes + itemSize > this.maxMemoryBytes))
|
|
{
|
|
return false; /* 若队满或超过内存限制则入队失败 */
|
|
}
|
|
|
|
const clone = this._deepClone(item); /* 深拷贝入队数据 (避免外部修改影响队列) */
|
|
this.buffer[this.tail] = clone; /* 存入队尾 */
|
|
this.tail = (this.tail + 1) % this.capacity; /* 环形增长 tail */
|
|
this.currentMemoryBytes += itemSize; /* 增加当前内存使用 */
|
|
|
|
if (this.tail === this.head) this.isFull = true; /* 若尾追头，表示队满 */
|
|
return true;
|
|
}
|
|
|
|
/* 出队操作：从队头取出一个对象, 返回深拷贝 */
|
|
out()
|
|
{
|
|
if (this.isEmpty()) return null; /* 队空返回 null */
|
|
|
|
const item = this.buffer[this.head]; /* 读取队头项目 */
|
|
this.currentMemoryBytes -= this._estimateSize(item); /* 更新内存使用 */
|
|
this.head = (this.head + 1) % this.capacity; /* 环形增长 head */
|
|
this.isFull = false; /* 出队必然队不满 */
|
|
return this._deepClone(item); /* 返回深拷贝，保护队列内部数据 */
|
|
}
|
|
|
|
/* 丢弃数据：从队尾向前丢弃 len 项 (反向移动 tail) */
|
|
discard(len)
|
|
{
|
|
for (let i = 0; i < len; i++)
|
|
{
|
|
if (this.isEmpty()) return false; /* 队空则不能继续丢弃 */
|
|
|
|
/* 定位最后一项 */
|
|
const item = this.buffer[(this.tail - 1 + this.capacity) % this.capacity];
|
|
this.currentMemoryBytes -= this._estimateSize(item); /* 减去内存大小 */
|
|
this.tail = (this.tail - 1 + this.capacity) % this.capacity; /* tail 回退 */
|
|
this.isFull = false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/* 获取当前队列中元素个数 */
|
|
getDepth()
|
|
{
|
|
return this.isFull
|
|
? this.capacity
|
|
: (this.tail + this.capacity - this.head) % this.capacity;
|
|
}
|
|
|
|
/**
|
|
* 异步入队，支持超时退出,不会阻塞主线程, 可用于异步场景
|
|
* buf : 需要发送的数据
|
|
* timeoutMs : 超时时间
|
|
*/
|
|
async send(item, timeoutMs = 0)
|
|
{
|
|
const start = Date.now();
|
|
|
|
/* 循环直到成功入队或超时 */
|
|
while (!this.enter(item))
|
|
{
|
|
if (Date.now() - start >= timeoutMs) return false; /* 超时退出 */
|
|
await new Promise(resolve => setTimeout(resolve, 1)); /* 异步轮询, 自旋加 sleep 挂起 1ms */
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* 异步出队，支持超时退出,不会阻塞主线程, 可用于异步场景
|
|
* buf : 需要发送的数据
|
|
* timeoutMs : 超时时间
|
|
* 返回值：成功取出一个对象(深拷贝)或 null (超时)
|
|
*/
|
|
async recv(timeoutMs = 0)
|
|
{
|
|
const start = Date.now();
|
|
while (true)
|
|
{
|
|
const item = this.out();
|
|
if (item !== null) return item; /* 成功取出则返回 */
|
|
if (Date.now() - start >= timeoutMs) return null; /* 超时退出 */
|
|
await new Promise(resolve => setTimeout(resolve, 1)); /* 异步轮询, 自旋加 sleep 挂起 1ms */
|
|
}
|
|
}
|
|
|
|
/* JSON 序列化接口，用于 JSON.stringify(queue) */
|
|
toJSON()
|
|
{
|
|
const result = [];
|
|
let i = this.head;
|
|
let count = this.getDepth();
|
|
while (count-- > 0)
|
|
{
|
|
result.push(this._deepClone(this.buffer[i])); /* 复制每个元素 */
|
|
i = (i + 1) % this.capacity; /* 移动到下一个元素 */
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
class TryLock
|
|
{
|
|
constructor()
|
|
{
|
|
this._locked = false;
|
|
}
|
|
/**
|
|
* 尝试获取锁
|
|
* true: 获取成功, false: 已被锁定
|
|
*/
|
|
tryLock()
|
|
{
|
|
if (this._locked)
|
|
{
|
|
return false;
|
|
}
|
|
this._locked = true;
|
|
return true;
|
|
}
|
|
|
|
/* 释放锁资源 */
|
|
release()
|
|
{
|
|
this._locked = false;
|
|
}
|
|
}
|
|
// export { TracePoint, TPAction, TPMode };
|
|
// module.exports = {
|
|
// TracePoint,
|
|
// TPAction,
|
|
// TPMode,
|
|
// };
|