f封装完成点 无人机和 机器人 地图清空实时轨迹和清空规划轨迹
This commit is contained in:
@@ -12,8 +12,8 @@
|
||||
* 轨迹模式: 实时绘制历史轨迹
|
||||
*/
|
||||
const TPMode = Object.freeze({
|
||||
NAVIGATION: Symbol('NAVIGATION'),
|
||||
LOCATION: Symbol('LOCATION'),
|
||||
NAVIGATION: Symbol('NAVIGATION'),
|
||||
LOCATION: Symbol('LOCATION'),
|
||||
TRACK: Symbol('TRACK'),
|
||||
});
|
||||
|
||||
@@ -24,17 +24,10 @@ const TPMode = Object.freeze({
|
||||
* ADD : 添加
|
||||
*/
|
||||
const TPAction = Object.freeze({
|
||||
UPDATE: Symbol('UPDATE'),
|
||||
ADD: Symbol('ADD'),
|
||||
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 类
|
||||
@@ -68,8 +61,7 @@ const TPAction = Object.freeze({
|
||||
* 4. 添加或更新点调用 upsert
|
||||
*
|
||||
*=============================================================================================================*/
|
||||
class TracePoint
|
||||
{
|
||||
class TracePoint {
|
||||
/* 成员变量(私有) */
|
||||
#queue; /* 循环队列, 用于存放等待处理的点 */
|
||||
#tracePoint; /* 轨迹点数组, 存放已完成点 + 当前点 */
|
||||
@@ -80,12 +72,10 @@ class TracePoint
|
||||
/* ========================
|
||||
* TEST ONLY: BEGIN
|
||||
* ========================*/
|
||||
tryLock()
|
||||
{
|
||||
tryLock() {
|
||||
return this.#lock.tryLock();
|
||||
}
|
||||
release()
|
||||
{
|
||||
release() {
|
||||
this.#lock.release();
|
||||
}
|
||||
/* ========================
|
||||
@@ -93,11 +83,10 @@ class TracePoint
|
||||
* ========================*/
|
||||
|
||||
/* 构造函数 */
|
||||
constructor()
|
||||
{
|
||||
this.#queue = new CircQueue(5, {
|
||||
constructor() {
|
||||
this.#queue = new CircQueue(100, {
|
||||
deepCopy: true,
|
||||
maxMemoryBytes: 1024*1024, /* 1MB 限制 */
|
||||
maxMemoryBytes: 1024 * 1024, /* 1MB 限制 */
|
||||
});
|
||||
|
||||
|
||||
@@ -108,12 +97,11 @@ class TracePoint
|
||||
}
|
||||
|
||||
/* 复位 */
|
||||
reset()
|
||||
{
|
||||
reset() {
|
||||
/* 重置轨迹点 */
|
||||
// let len = this.#queue.getDepth();
|
||||
// this.#queue.discard(len);
|
||||
this.#queue.clear(); /* 清空队列 */
|
||||
// let len = this.#queue.getDepth();
|
||||
// this.#queue.discard(len);
|
||||
this.#queue.clear(); /* 清空队列 */
|
||||
this.#tracePoint = []; /* 清空轨迹点数组 */
|
||||
this.#completePointIndex = 0; /* 重置完成点索引 */
|
||||
this.#lock.release(); /* 释放锁 */
|
||||
@@ -123,80 +111,69 @@ class TracePoint
|
||||
* mode: 模式选择器, 见枚举 TPMode
|
||||
* 说明:
|
||||
* 导航模式: 绘制规划已完成路径和当前路径
|
||||
* 定位模式: 绘制当前点
|
||||
* 轨迹模式: 实时绘制历史轨迹
|
||||
* 定位模式: 绘制当前点
|
||||
* 轨迹模式: 实时绘制历史轨迹
|
||||
*/
|
||||
setMode(mode)
|
||||
{
|
||||
setMode(mode) {
|
||||
this.reset();
|
||||
this.#mode = mode;
|
||||
}
|
||||
getMode() {
|
||||
return this.#mode;
|
||||
}
|
||||
|
||||
/**
|
||||
* point : 需要添加或更细的点
|
||||
* act : 仅导航模式下有效, 缺省为 TPAction.UPDATE , 具体如下:
|
||||
* 1. 添加完成点时调用: upsert(point, TPAction.ADD)
|
||||
* 2. 更新当前点时调用: upsert(point)
|
||||
* 2. 更新当前点时调用: upsert(point)
|
||||
*/
|
||||
upsert(point, act = TPAction.UPDATE)
|
||||
{
|
||||
if(this.#mode === TPMode.NAVIGATION)
|
||||
{
|
||||
(act === TPAction.UPDATE)
|
||||
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)
|
||||
{
|
||||
else if (this.#mode === TPMode.LOCATION) {
|
||||
this.#updateCurrentPoint(point);
|
||||
}
|
||||
else if(this.#mode === TPMode.TRACK)
|
||||
{
|
||||
else if (this.#mode === TPMode.TRACK) {
|
||||
this.#addCompletePoint(point);
|
||||
}
|
||||
}
|
||||
|
||||
/* 获取轨迹 */
|
||||
getTracePoint()
|
||||
{
|
||||
getTracePoint() {
|
||||
/* 尝试锁定, 防止多线程同时更新 */
|
||||
if( this.#lock.tryLock() )
|
||||
{
|
||||
if (this.#lock.tryLock()) {
|
||||
const trace = this.#tracePoint.slice(); /* 深拷贝, 避免外部修改导致内部出错 */
|
||||
this.#lock.release(); /* 解锁 */
|
||||
return trace;
|
||||
}
|
||||
else
|
||||
{
|
||||
else {
|
||||
console.warn('getTracePoint: lock!!!');
|
||||
return null; /* 锁定中, 返回空 */
|
||||
}
|
||||
}
|
||||
|
||||
/* 更新当前点 */
|
||||
#updateCurrentPoint(point)
|
||||
{
|
||||
if (!point)
|
||||
{
|
||||
#updateCurrentPoint(point) {
|
||||
if (!point) {
|
||||
console.error('updateCurrentPoint: point is NULL');
|
||||
return;
|
||||
}
|
||||
|
||||
/* 尝试锁定, 防止多线程同时更新 */
|
||||
if( this.#lock.tryLock() )
|
||||
{
|
||||
if (this.#lock.tryLock()) {
|
||||
/**
|
||||
* 导航模式: 当前点只插在尾部,若轨迹数组尚未插入完成点, 则丢弃
|
||||
* 定位模式: 轨迹数组只有一个点
|
||||
*/
|
||||
if(this.#mode === TPMode.NAVIGATION)
|
||||
{
|
||||
if (this.#tracePoint.length === 0)
|
||||
{
|
||||
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
|
||||
{
|
||||
else {
|
||||
/* 只保留完成点部分, 再插入新的当前点 */
|
||||
this.#tracePoint = this.#tracePoint.slice(0, this.#completePointIndex);
|
||||
this.#tracePoint.push(point);
|
||||
@@ -205,38 +182,31 @@ class TracePoint
|
||||
// emit
|
||||
}
|
||||
}
|
||||
else if(this.#mode === TPMode.LOCATION)
|
||||
{
|
||||
else if (this.#mode === TPMode.LOCATION) {
|
||||
this.#tracePoint[0] = point; /* JS 支持自动扩展数组 */
|
||||
}
|
||||
|
||||
this.#lock.release(); /* 解锁 */
|
||||
}
|
||||
/* 如果轨迹数组正在操作, 先存放至队列中 */
|
||||
else
|
||||
{
|
||||
else {
|
||||
console.warn('updateCurrentPoint: lock!!!, discard current point:', point);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* 添加完成点 */
|
||||
#addCompletePoint(point)
|
||||
{
|
||||
if (!point)
|
||||
{
|
||||
#addCompletePoint(point) {
|
||||
if (!point) {
|
||||
console.error('addCompletePoint: point is NULL');
|
||||
return;
|
||||
}
|
||||
|
||||
/* 尝试锁定, 防止多线程同时更新 */
|
||||
if( this.#lock.tryLock() )
|
||||
{
|
||||
try
|
||||
{
|
||||
if (this.#lock.tryLock()) {
|
||||
try {
|
||||
/* 先处理队列中积压的点 */
|
||||
while (this.#queue.isEmpty() === false)
|
||||
{
|
||||
while (this.#queue.isEmpty() === false) {
|
||||
/* 如果队列不为空, 取出队列中的点 */
|
||||
const point = this.#queue.out();
|
||||
this.#addPoint(point);
|
||||
@@ -248,31 +218,26 @@ class TracePoint
|
||||
/* 触发事件 newCompletePoint, 驱动外部更新 */
|
||||
// emit
|
||||
}
|
||||
catch (error)
|
||||
{
|
||||
catch (error) {
|
||||
/* 错误处理, js 可以输出到 .err 文件? */
|
||||
|
||||
/* 打印 */
|
||||
console.error('addCompletePoint failure:', error);
|
||||
}
|
||||
finally
|
||||
{
|
||||
finally {
|
||||
this.#lock.release(); /* 无论成功失败都解锁, 避免无法使用 */
|
||||
}
|
||||
}
|
||||
/* 如果轨迹数组正在操作, 先存放至队列中 */
|
||||
else
|
||||
{
|
||||
else {
|
||||
this.#queue.enter(point);
|
||||
console.warn('addCompletePoint: lock!!!, enter queue, wait disposing:', point);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
#addPoint(point)
|
||||
{
|
||||
if (!point)
|
||||
{
|
||||
#addPoint(point) {
|
||||
if (!point) {
|
||||
console.error('point NULL:');
|
||||
return;
|
||||
}
|
||||
@@ -280,15 +245,13 @@ class TracePoint
|
||||
/* 若当前点存在,说明当前数组末尾已插入一个当前点, 截断轨迹数组到当前点,将新完成点插入在当前点之前 */
|
||||
const hasCurrentPoint = (this.#tracePoint.length > 0 && this.#completePointIndex < this.#tracePoint.length);
|
||||
|
||||
if (hasCurrentPoint)
|
||||
{
|
||||
if (hasCurrentPoint) {
|
||||
/* 截断避免多添加了无效点, 然后再当前点之前插入完成点 */
|
||||
this.#tracePoint = this.#tracePoint.slice(0, this.#completePointIndex);
|
||||
this.#tracePoint.splice(this.#completePointIndex, 0, point);
|
||||
}
|
||||
/* 首次添加轨迹点需要先尾插 */
|
||||
else
|
||||
{
|
||||
else {
|
||||
this.#tracePoint.push(point);
|
||||
}
|
||||
|
||||
@@ -297,8 +260,7 @@ class TracePoint
|
||||
}
|
||||
}
|
||||
|
||||
class CircQueue
|
||||
{
|
||||
class CircQueue {
|
||||
/**
|
||||
* 构造函数
|
||||
* capacity :队列可容纳的最大项目数
|
||||
@@ -307,8 +269,7 @@ class CircQueue
|
||||
* maxMemoryBytes :队列的最大内存限制 (单位:字节),默认 Infinity, 不限制内存
|
||||
* 若传 1024 * 1024 代表限制 1 M ,超出则队满, 队列会拒绝入队
|
||||
*/
|
||||
constructor(capacity, options = {})
|
||||
{
|
||||
constructor(capacity, options = {}) {
|
||||
this.capacity = capacity; /* 队列容量 (最多存储项目数) */
|
||||
this.buffer = new Array(capacity); /* 队列存储区,使用数组存任意对象 */
|
||||
this.head = 0; /* 队头索引 */
|
||||
@@ -320,50 +281,41 @@ class CircQueue
|
||||
this.currentMemoryBytes = 0; /* 当前已使用内存大小 (单位:字节) */
|
||||
}
|
||||
|
||||
/* 清空队列 */
|
||||
clear()
|
||||
{
|
||||
this.tail = this.head;
|
||||
this.isFull = false;
|
||||
}
|
||||
/* 清空队列 */
|
||||
clear() {
|
||||
this.tail = this.head;
|
||||
this.isFull = false;
|
||||
}
|
||||
|
||||
/* 判断队列是否为空 */
|
||||
isEmpty()
|
||||
{
|
||||
isEmpty() {
|
||||
return this.head === this.tail && !this.isFull;
|
||||
}
|
||||
|
||||
/* 判断队列是否已满 */
|
||||
isFullFn()
|
||||
{
|
||||
isFullFn() {
|
||||
return this.isFull;
|
||||
}
|
||||
|
||||
/* 估算对象大小 (以 JSON 字符串的 UTF-8 编码长度作为近似值) */
|
||||
_estimateSize(obj)
|
||||
{
|
||||
try
|
||||
{
|
||||
return Buffer.byteLength(JSON.stringify(obj), 'utf8'); /* Node.js 中用于计算字节长度 */
|
||||
_estimateSize(obj) {
|
||||
try {
|
||||
return new Blob([JSON.stringify(obj)]).size;
|
||||
}
|
||||
catch
|
||||
{
|
||||
catch {
|
||||
return 0; /* 若对象不可序列化,返回 0 (避免出错) */
|
||||
}
|
||||
}
|
||||
|
||||
/* 深拷贝对象 (基于 JSON 的方式,仅支持可序列化对象) */
|
||||
_deepClone(obj)
|
||||
{
|
||||
_deepClone(obj) {
|
||||
return this.deepCopy ? JSON.parse(JSON.stringify(obj)) : obj;
|
||||
}
|
||||
|
||||
/* 入队操作:将对象放入队尾 */
|
||||
enter(item)
|
||||
{
|
||||
enter(item) {
|
||||
const itemSize = this._estimateSize(item); /* 估算项目内存大小 */
|
||||
if (this.isFullFn() || (this.currentMemoryBytes + itemSize > this.maxMemoryBytes))
|
||||
{
|
||||
if (this.isFullFn() || (this.currentMemoryBytes + itemSize > this.maxMemoryBytes)) {
|
||||
return false; /* 若队满或超过内存限制则入队失败 */
|
||||
}
|
||||
|
||||
@@ -377,8 +329,7 @@ class CircQueue
|
||||
}
|
||||
|
||||
/* 出队操作:从队头取出一个对象, 返回深拷贝 */
|
||||
out()
|
||||
{
|
||||
out() {
|
||||
if (this.isEmpty()) return null; /* 队空返回 null */
|
||||
|
||||
const item = this.buffer[this.head]; /* 读取队头项目 */
|
||||
@@ -389,10 +340,8 @@ class CircQueue
|
||||
}
|
||||
|
||||
/* 丢弃数据:从队尾向前丢弃 len 项 (反向移动 tail) */
|
||||
discard(len)
|
||||
{
|
||||
for (let i = 0; i < len; i++)
|
||||
{
|
||||
discard(len) {
|
||||
for (let i = 0; i < len; i++) {
|
||||
if (this.isEmpty()) return false; /* 队空则不能继续丢弃 */
|
||||
|
||||
/* 定位最后一项 */
|
||||
@@ -405,8 +354,7 @@ class CircQueue
|
||||
}
|
||||
|
||||
/* 获取当前队列中元素个数 */
|
||||
getDepth()
|
||||
{
|
||||
getDepth() {
|
||||
return this.isFull
|
||||
? this.capacity
|
||||
: (this.tail + this.capacity - this.head) % this.capacity;
|
||||
@@ -417,13 +365,11 @@ class CircQueue
|
||||
* buf : 需要发送的数据
|
||||
* timeoutMs : 超时时间
|
||||
*/
|
||||
async send(item, timeoutMs = 0)
|
||||
{
|
||||
async send(item, timeoutMs = 0) {
|
||||
const start = Date.now();
|
||||
|
||||
/* 循环直到成功入队或超时 */
|
||||
while (!this.enter(item))
|
||||
{
|
||||
while (!this.enter(item)) {
|
||||
if (Date.now() - start >= timeoutMs) return false; /* 超时退出 */
|
||||
await new Promise(resolve => setTimeout(resolve, 1)); /* 异步轮询, 自旋加 sleep 挂起 1ms */
|
||||
}
|
||||
@@ -436,11 +382,9 @@ class CircQueue
|
||||
* timeoutMs : 超时时间
|
||||
* 返回值:成功取出一个对象(深拷贝)或 null (超时)
|
||||
*/
|
||||
async recv(timeoutMs = 0)
|
||||
{
|
||||
async recv(timeoutMs = 0) {
|
||||
const start = Date.now();
|
||||
while (true)
|
||||
{
|
||||
while (true) {
|
||||
const item = this.out();
|
||||
if (item !== null) return item; /* 成功取出则返回 */
|
||||
if (Date.now() - start >= timeoutMs) return null; /* 超时退出 */
|
||||
@@ -449,33 +393,27 @@ class CircQueue
|
||||
}
|
||||
|
||||
/* JSON 序列化接口,用于 JSON.stringify(queue) */
|
||||
toJSON()
|
||||
{
|
||||
toJSON() {
|
||||
const result = [];
|
||||
let i = this.head;
|
||||
let count = this.getDepth();
|
||||
while (count-- > 0)
|
||||
{
|
||||
while (count-- > 0) {
|
||||
result.push(this._deepClone(this.buffer[i])); /* 复制每个元素 */
|
||||
i = (i + 1) % this.capacity; /* 移动到下一个元素 */
|
||||
}
|
||||
return result;
|
||||
}
|
||||
}
|
||||
class TryLock
|
||||
{
|
||||
constructor()
|
||||
{
|
||||
class TryLock {
|
||||
constructor() {
|
||||
this._locked = false;
|
||||
}
|
||||
/**
|
||||
* 尝试获取锁
|
||||
* true: 获取成功, false: 已被锁定
|
||||
*/
|
||||
tryLock()
|
||||
{
|
||||
if (this._locked)
|
||||
{
|
||||
tryLock() {
|
||||
if (this._locked) {
|
||||
return false;
|
||||
}
|
||||
this._locked = true;
|
||||
@@ -483,14 +421,8 @@ class TryLock
|
||||
}
|
||||
|
||||
/* 释放锁资源 */
|
||||
release()
|
||||
{
|
||||
release() {
|
||||
this._locked = false;
|
||||
}
|
||||
}
|
||||
// export { TracePoint, TPAction, TPMode };
|
||||
// module.exports = {
|
||||
// TracePoint,
|
||||
// TPAction,
|
||||
// TPMode,
|
||||
// };
|
||||
export { TracePoint, TPAction, TPMode };
|
||||
|
||||
Reference in New Issue
Block a user