路径规划修复
This commit is contained in:
@@ -5,7 +5,7 @@ import 'dart:async';
|
||||
/// 轨迹模式枚举
|
||||
enum TPMode {
|
||||
NAVIGATION, // 导航模式:绘制规划已完成路径和当前路径
|
||||
LOCATION, // 定位模式:仅绘制当前点
|
||||
LOCATION, // 定位模式:仅绘制当前点(永远只有1个)
|
||||
TRACK, // 轨迹模式:实时绘制历史轨迹
|
||||
}
|
||||
|
||||
@@ -19,58 +19,46 @@ enum TPAction {
|
||||
class TryLock {
|
||||
bool _locked = false;
|
||||
|
||||
/// 尝试获取锁
|
||||
/// return: true-获取成功,false-已被锁定
|
||||
bool tryLock() {
|
||||
if (_locked) return false;
|
||||
_locked = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// 释放锁
|
||||
void release() {
|
||||
_locked = false;
|
||||
}
|
||||
|
||||
/// 检查是否已锁定
|
||||
bool get isLocked => _locked;
|
||||
}
|
||||
|
||||
// ======================== 环形队列实现 ========================
|
||||
class CircQueue<T> {
|
||||
final int capacity; // 队列容量
|
||||
final List<T?> buffer; // 存储缓冲区
|
||||
int head = 0; // 队头索引
|
||||
int tail = 0; // 队尾索引
|
||||
bool isFull = false; // 队列满标志
|
||||
final bool deepCopy; // 是否深拷贝元素
|
||||
final int maxMemoryBytes; // 最大内存限制(字节)
|
||||
int currentMemoryBytes = 0; // 当前已使用内存
|
||||
final int capacity;
|
||||
final List<T?> buffer;
|
||||
int head = 0;
|
||||
int tail = 0;
|
||||
bool isFull = false;
|
||||
final bool deepCopy;
|
||||
final int maxMemoryBytes;
|
||||
int currentMemoryBytes = 0;
|
||||
|
||||
CircQueue(
|
||||
this.capacity, {
|
||||
this.deepCopy = true,
|
||||
this.maxMemoryBytes = 1024 * 1024, // 默认1MB
|
||||
}) : buffer = List<T?>.filled(capacity, null);
|
||||
CircQueue(this.capacity, {this.deepCopy = true, this.maxMemoryBytes = 1024 * 1024}) : buffer = List<T?>.filled(capacity, null);
|
||||
|
||||
/// 清空队列
|
||||
void clear() {
|
||||
head = tail;
|
||||
isFull = false;
|
||||
currentMemoryBytes = 0;
|
||||
}
|
||||
|
||||
/// 判断队列是否为空
|
||||
bool isEmpty() {
|
||||
return head == tail && !isFull;
|
||||
}
|
||||
|
||||
/// 判断队列是否已满
|
||||
bool isFullFn() {
|
||||
return isFull;
|
||||
}
|
||||
|
||||
/// 估算对象大小(JSON序列化后的字节长度)
|
||||
int _estimateSize(T item) {
|
||||
try {
|
||||
return utf8.encode(jsonEncode(item)).length;
|
||||
@@ -79,7 +67,6 @@ class CircQueue<T> {
|
||||
}
|
||||
}
|
||||
|
||||
/// 深拷贝对象
|
||||
T? _deepClone(T? item) {
|
||||
if (!deepCopy || item == null) return item;
|
||||
try {
|
||||
@@ -89,10 +76,8 @@ class CircQueue<T> {
|
||||
}
|
||||
}
|
||||
|
||||
/// 入队操作
|
||||
bool enter(T item) {
|
||||
final itemSize = _estimateSize(item);
|
||||
// 检查队列满或内存超限
|
||||
if (isFullFn() || (currentMemoryBytes + itemSize > maxMemoryBytes)) {
|
||||
return false;
|
||||
}
|
||||
@@ -106,7 +91,6 @@ class CircQueue<T> {
|
||||
return true;
|
||||
}
|
||||
|
||||
/// 出队操作
|
||||
T? out() {
|
||||
if (isEmpty()) return null;
|
||||
|
||||
@@ -119,7 +103,6 @@ class CircQueue<T> {
|
||||
return _deepClone(item);
|
||||
}
|
||||
|
||||
/// 丢弃指定数量的元素
|
||||
bool discard(int len) {
|
||||
for (int i = 0; i < len; i++) {
|
||||
if (isEmpty()) return false;
|
||||
@@ -135,12 +118,10 @@ class CircQueue<T> {
|
||||
return true;
|
||||
}
|
||||
|
||||
/// 获取队列元素个数
|
||||
int getDepth() {
|
||||
return isFull ? capacity : (tail + capacity - head) % capacity;
|
||||
}
|
||||
|
||||
/// 异步入队(支持超时)
|
||||
Future<bool> send(T item, {int timeoutMs = 0}) async {
|
||||
final start = DateTime.now().millisecondsSinceEpoch;
|
||||
while (!enter(item)) {
|
||||
@@ -152,7 +133,6 @@ class CircQueue<T> {
|
||||
return true;
|
||||
}
|
||||
|
||||
/// 异步出队(支持超时)
|
||||
Future<T?> recv({int timeoutMs = 0}) async {
|
||||
final start = DateTime.now().millisecondsSinceEpoch;
|
||||
while (true) {
|
||||
@@ -165,7 +145,6 @@ class CircQueue<T> {
|
||||
}
|
||||
}
|
||||
|
||||
/// 转换为列表(用于调试/序列化)
|
||||
List<T?> toList() {
|
||||
final result = <T?>[];
|
||||
int i = head;
|
||||
@@ -179,22 +158,18 @@ class CircQueue<T> {
|
||||
}
|
||||
|
||||
class TracePoint<T> {
|
||||
// 私有成员变量
|
||||
late final CircQueue<T> _queue;
|
||||
List<T> _tracePoint = [];
|
||||
int _completePointIndex = 0;
|
||||
late final TryLock _lock;
|
||||
TPMode _mode = TPMode.LOCATION;
|
||||
|
||||
// 公开的回调(无下划线,外部可访问)
|
||||
void Function(T)? onCurrentPointUpdated;
|
||||
void Function(T)? onCompletePointAdded;
|
||||
|
||||
// 测试用方法
|
||||
bool tryLock() => _lock.tryLock();
|
||||
void release() => _lock.release();
|
||||
|
||||
/// 构造函数
|
||||
TracePoint({int queueCapacity = 5, int maxMemoryBytes = 1024 * 1024}) {
|
||||
_queue = CircQueue<T>(queueCapacity, deepCopy: true, maxMemoryBytes: maxMemoryBytes);
|
||||
_lock = TryLock();
|
||||
@@ -204,12 +179,12 @@ class TracePoint<T> {
|
||||
/// 复位所有状态
|
||||
void reset() {
|
||||
_queue.clear();
|
||||
_tracePoint = [];
|
||||
_tracePoint.clear();
|
||||
_completePointIndex = 0;
|
||||
_lock.release();
|
||||
}
|
||||
|
||||
/// 设置工作模式
|
||||
/// 设置模式
|
||||
void setMode(TPMode mode) {
|
||||
reset();
|
||||
_mode = mode;
|
||||
@@ -221,20 +196,29 @@ class TracePoint<T> {
|
||||
|
||||
/// 添加/更新轨迹点
|
||||
void upsert(T point, [TPAction act = TPAction.UPDATE]) {
|
||||
// 严格按模式分发
|
||||
switch (_mode) {
|
||||
case TPMode.NAVIGATION:
|
||||
act == TPAction.UPDATE ? _updateCurrentPoint(point) : _addCompletePoint(point);
|
||||
if (act == TPAction.UPDATE) {
|
||||
_updateCurrentPoint(point);
|
||||
} else {
|
||||
_addCompletePoint(point);
|
||||
}
|
||||
break;
|
||||
|
||||
case TPMode.LOCATION:
|
||||
// LOCATION 永远只保留最新一个点
|
||||
_updateCurrentPoint(point);
|
||||
break;
|
||||
|
||||
case TPMode.TRACK:
|
||||
// TRACK 每个点都直接添加,不进队列
|
||||
_addCompletePoint(point);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/// 获取轨迹点列表(深拷贝,线程安全)
|
||||
/// 获取轨迹列表
|
||||
List<T>? getTracePoint() {
|
||||
if (_lock.tryLock()) {
|
||||
final trace = List<T>.from(_tracePoint);
|
||||
@@ -246,63 +230,48 @@ class TracePoint<T> {
|
||||
}
|
||||
}
|
||||
|
||||
/// 更新当前点(私有方法)
|
||||
/// 更新当前点(LOCATION 永远只有1个)
|
||||
void _updateCurrentPoint(T point) {
|
||||
if (point == null) {
|
||||
print('updateCurrentPoint: point is NULL');
|
||||
return;
|
||||
}
|
||||
|
||||
if (_lock.tryLock()) {
|
||||
try {
|
||||
switch (_mode) {
|
||||
case TPMode.NAVIGATION:
|
||||
if (_tracePoint.isEmpty) {
|
||||
print('updateCurrentPoint: tracePoint is empty , wait first completed point, discard current point: $point');
|
||||
} else {
|
||||
if (_tracePoint.isNotEmpty) {
|
||||
_tracePoint = _tracePoint.sublist(0, _completePointIndex);
|
||||
_tracePoint.add(point);
|
||||
// 触发公开的回调(无下划线)
|
||||
onCurrentPointUpdated?.call(point);
|
||||
}
|
||||
break;
|
||||
|
||||
case TPMode.LOCATION:
|
||||
if (_tracePoint.isEmpty) {
|
||||
_tracePoint.add(point);
|
||||
} else {
|
||||
_tracePoint[0] = point;
|
||||
}
|
||||
// 触发公开的回调(无下划线)
|
||||
// 核心:永远清空,只保留最新一个
|
||||
_tracePoint.clear();
|
||||
_tracePoint.add(point);
|
||||
onCurrentPointUpdated?.call(point);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} finally {
|
||||
_lock.release();
|
||||
}
|
||||
} else {
|
||||
print('updateCurrentPoint: lock!!!, discard current point: $point');
|
||||
}
|
||||
}
|
||||
|
||||
/// 添加完成点(私有方法)
|
||||
/// 添加完成点
|
||||
void _addCompletePoint(T point) {
|
||||
if (point == null) {
|
||||
print('addCompletePoint: point is NULL');
|
||||
return;
|
||||
}
|
||||
|
||||
if (_lock.tryLock()) {
|
||||
try {
|
||||
// 先消费队列
|
||||
while (!_queue.isEmpty()) {
|
||||
final queuedPoint = _queue.out();
|
||||
if (queuedPoint != null) {
|
||||
_addPoint(queuedPoint);
|
||||
}
|
||||
}
|
||||
// 添加当前点
|
||||
_addPoint(point);
|
||||
// 触发公开的回调(无下划线)
|
||||
onCompletePointAdded?.call(point);
|
||||
} catch (e) {
|
||||
print('addCompletePoint failure: $e');
|
||||
@@ -310,23 +279,29 @@ class TracePoint<T> {
|
||||
_lock.release();
|
||||
}
|
||||
} else {
|
||||
_queue.enter(point);
|
||||
print('addCompletePoint: lock!!!, enter queue, wait disposing: $point');
|
||||
if (_mode != TPMode.TRACK) {
|
||||
// TRACK 模式不加队列,直接丢弃旧的保证流畅
|
||||
_queue.enter(point);
|
||||
}
|
||||
print('addCompletePoint: lock!!!');
|
||||
}
|
||||
}
|
||||
|
||||
/// 内部添加点逻辑(私有方法)
|
||||
/// 内部添加点
|
||||
void _addPoint(T point) {
|
||||
if (point == null) {
|
||||
print('point NULL:');
|
||||
if (_mode == TPMode.LOCATION) {
|
||||
// LOCATION 强制只保留1个
|
||||
_tracePoint.clear();
|
||||
_tracePoint.add(point);
|
||||
_completePointIndex = 1;
|
||||
return;
|
||||
}
|
||||
|
||||
final hasCurrentPoint = _tracePoint.isNotEmpty && _completePointIndex < _tracePoint.length;
|
||||
final hasCurrent = _tracePoint.isNotEmpty && _completePointIndex < _tracePoint.length;
|
||||
|
||||
if (hasCurrentPoint) {
|
||||
if (hasCurrent) {
|
||||
_tracePoint = _tracePoint.sublist(0, _completePointIndex);
|
||||
_tracePoint.insert(_completePointIndex, point);
|
||||
_tracePoint.add(point);
|
||||
} else {
|
||||
_tracePoint.add(point);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user