f封装完成点 无人机和 机器人 地图清空实时轨迹和清空规划轨迹

This commit is contained in:
mmc
2026-06-11 17:02:23 +08:00
parent c9bc863354
commit 26dadc95cf
6 changed files with 586 additions and 265 deletions

View File

@@ -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 };