chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,22 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// 提供一次性障碍物快照的纯数据来源。
|
||||
///
|
||||
/// 注意：<see cref="ProjectToWorld"/> 不得读取传感器、定位、UI 或时钟；调用前应由上层采集并封装完整快照。
|
||||
/// </summary>
|
||||
public interface IMapObstacleSource
|
||||
{
|
||||
/// <summary>来源的稳定唯一标识。不能为空；同一次建图请求中不得重复。</summary>
|
||||
string SourceId { get; }
|
||||
/// <summary>来源快照版本号，必须非负。快照几何或有效性变化时必须递增，以使输入缓存失效。</summary>
|
||||
long SourceVersion { get; }
|
||||
/// <summary>该来源是否必需。true 时不可用或无效会使整张地图构建失败；false 时只记录状态。</summary>
|
||||
bool IsRequired { get; }
|
||||
/// <summary>
|
||||
/// 将已采集的快照投影为世界障碍物几何。
|
||||
///
|
||||
/// 返回：世界坐标、单位 mm 的障碍物及其状态；不得访问任何实时外部状态。
|
||||
/// </summary>
|
||||
ObstacleProjectionResult ProjectToWorld();
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>直接提供世界 mm 几何列表的手工障碍物快照来源。</summary>
|
||||
public sealed class ManualObstacleSource : IMapObstacleSource
|
||||
{
|
||||
private readonly IReadOnlyList<IMapObstacle> _obstacles;
|
||||
/// <summary>
|
||||
/// 创建手工障碍物来源。
|
||||
///
|
||||
/// 参数：sourceId 为唯一来源标识；sourceVersion 为非负快照版本；isRequired 指示失败是否阻断建图；obstacles 为世界 mm 几何列表。
|
||||
/// 注意：几何内容变化时，调用方必须同步提高 sourceVersion。
|
||||
/// </summary>
|
||||
public ManualObstacleSource(string sourceId, long sourceVersion, bool isRequired, IReadOnlyList<IMapObstacle> obstacles)
|
||||
{
|
||||
SourceId = sourceId; SourceVersion = sourceVersion; IsRequired = isRequired; _obstacles = obstacles;
|
||||
}
|
||||
/// <summary>该快照的唯一来源标识。</summary>
|
||||
public string SourceId { get; }
|
||||
/// <summary>该快照的版本号；内容变化时必须递增。</summary>
|
||||
public long SourceVersion { get; }
|
||||
/// <summary>true 表示无效来源会阻断建图；false 表示只记录来源状态。</summary>
|
||||
public bool IsRequired { get; }
|
||||
/// <summary>
|
||||
/// 返回手工障碍物的世界几何。
|
||||
///
|
||||
/// 返回：列表为空时为 Empty；列表为空引用或含无效几何时为 Invalid；其余情况为 Applied。
|
||||
/// </summary>
|
||||
public ObstacleProjectionResult ProjectToWorld()
|
||||
{
|
||||
if (_obstacles == null) return ObstacleProjectionResult.Invalid("Manual obstacle collection is null.");
|
||||
if (_obstacles.Count == 0) return ObstacleProjectionResult.Empty("Manual obstacle collection is empty.");
|
||||
for (int i = 0; i < _obstacles.Count; i++)
|
||||
if (_obstacles[i] == null || !_obstacles[i].IsValid) return ObstacleProjectionResult.Invalid("Manual obstacle geometry is invalid.");
|
||||
return ObstacleProjectionResult.Applied(_obstacles, "Manual obstacle snapshot applied.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>障碍物来源的一次投影结果，包含状态、世界 mm 几何和可选诊断信息。</summary>
|
||||
public sealed class ObstacleProjectionResult
|
||||
{
|
||||
private ObstacleProjectionResult(ObstacleSourceStatus status, IReadOnlyList<IMapObstacle> obstacles, string diagnostic)
|
||||
{
|
||||
Status = status; Obstacles = obstacles ?? Array.Empty<IMapObstacle>(); Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
/// <summary>投影状态。只有 <see cref="ObstacleSourceStatus.Applied"/> 的几何会被栅格化。</summary>
|
||||
public ObstacleSourceStatus Status { get; }
|
||||
/// <summary>世界 mm 坐标的障碍物列表。非 Applied 状态时为空列表，不为 null。</summary>
|
||||
public IReadOnlyList<IMapObstacle> Obstacles { get; }
|
||||
/// <summary>用于日志和诊断的说明文本；未提供时为空字符串。</summary>
|
||||
public string Diagnostic { get; }
|
||||
/// <summary>创建成功投影结果。参数 obstacles 为有效世界 mm 几何；diagnostic 为可选诊断文本。返回的状态为 Applied。</summary>
|
||||
public static ObstacleProjectionResult Applied(IReadOnlyList<IMapObstacle> obstacles, string diagnostic = null)
|
||||
{
|
||||
return new ObstacleProjectionResult(ObstacleSourceStatus.Applied, obstacles, diagnostic);
|
||||
}
|
||||
/// <summary>创建空投影结果。参数 diagnostic 为可选原因；返回状态为 Empty，障碍物列表为空。</summary>
|
||||
public static ObstacleProjectionResult Empty(string diagnostic = null) { return new ObstacleProjectionResult(ObstacleSourceStatus.Empty, null, diagnostic); }
|
||||
/// <summary>创建不可用投影结果。参数 diagnostic 应说明快照缺失原因；返回状态为 Unavailable。</summary>
|
||||
public static ObstacleProjectionResult Unavailable(string diagnostic) { return new ObstacleProjectionResult(ObstacleSourceStatus.Unavailable, null, diagnostic); }
|
||||
/// <summary>创建无效投影结果。参数 diagnostic 应说明数据或几何错误；返回状态为 Invalid。</summary>
|
||||
public static ObstacleProjectionResult Invalid(string diagnostic) { return new ObstacleProjectionResult(ObstacleSourceStatus.Invalid, null, diagnostic); }
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>一个障碍物来源投影后的状态，供建图结果和调试日志判断。</summary>
|
||||
public enum ObstacleSourceStatus
|
||||
{
|
||||
/// <summary>成功得到一个或多个有效的世界 mm 障碍物，并会被栅格化。</summary>
|
||||
Applied,
|
||||
/// <summary>来源有效但本次没有障碍物；不会写入任何栅格。</summary>
|
||||
Empty,
|
||||
/// <summary>来源快照不可取得；若来源必需则整图构建失败。</summary>
|
||||
Unavailable,
|
||||
/// <summary>来源数据或几何无效；若来源必需则整图构建失败。</summary>
|
||||
Invalid,
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>将检测局部坐标中的两条腿转换为世界 mm 坐标圆形障碍物。</summary>
|
||||
public static class TwoLegObstacleProjector
|
||||
{
|
||||
/// <summary>
|
||||
/// 投影一份 TwoLeg 快照。
|
||||
///
|
||||
/// 参数：input 为检测时刻采集的纯数据快照，可为 null。
|
||||
/// 返回：null 为 Unavailable；未检测到目标为 Empty；数值无效为 Invalid；成功时为含两个圆形障碍物的 Applied。
|
||||
/// 注意：本方法只读取 input，不访问传感器、定位、UI 或时钟。
|
||||
/// </summary>
|
||||
public static ObstacleProjectionResult Project(TwoLegProjectionInput input)
|
||||
{
|
||||
if (input == null) return ObstacleProjectionResult.Unavailable("TwoLeg snapshot is unavailable.");
|
||||
if (!input.HasDetection) return ObstacleProjectionResult.Empty(input.Diagnostic);
|
||||
if (!input.IsValid) return ObstacleProjectionResult.Invalid("TwoLeg snapshot contains invalid values.");
|
||||
CoordinateTransform.LocalToWorld(input.DetectionWorldX, input.DetectionWorldY, input.DetectionHeadingRadians,
|
||||
input.FirstLocalX, input.FirstLocalY, out double firstX, out double firstY);
|
||||
CoordinateTransform.LocalToWorld(input.DetectionWorldX, input.DetectionWorldY, input.DetectionHeadingRadians,
|
||||
input.SecondLocalX, input.SecondLocalY, out double secondX, out double secondY);
|
||||
var obstacles = new List<IMapObstacle>
|
||||
{
|
||||
new CircleObstacle((float)firstX, (float)firstY, input.RadiusMm),
|
||||
new CircleObstacle((float)secondX, (float)secondY, input.RadiusMm),
|
||||
};
|
||||
return ObstacleProjectionResult.Applied(obstacles, input.Diagnostic);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>将一个 <see cref="TwoLegProjectionInput"/> 快照包装为统一障碍物来源。</summary>
|
||||
public sealed class TwoLegObstacleSource : IMapObstacleSource
|
||||
{
|
||||
private readonly TwoLegProjectionInput _input;
|
||||
/// <summary>
|
||||
/// 创建 TwoLeg 障碍物来源。
|
||||
///
|
||||
/// 参数：sourceId 为唯一标识；sourceVersion 为快照版本，输入内容改变时必须递增；isRequired 指示失败是否阻断建图；input 为检测时刻快照，可为空。
|
||||
/// </summary>
|
||||
public TwoLegObstacleSource(string sourceId, long sourceVersion, bool isRequired, TwoLegProjectionInput input)
|
||||
{
|
||||
SourceId = sourceId; SourceVersion = sourceVersion; IsRequired = isRequired; _input = input;
|
||||
}
|
||||
/// <summary>该快照来源的唯一标识。</summary>
|
||||
public string SourceId { get; }
|
||||
/// <summary>快照版本号；输入内容变化时必须递增。</summary>
|
||||
public long SourceVersion { get; }
|
||||
/// <summary>true 时投影不可用或无效会阻断建图。</summary>
|
||||
public bool IsRequired { get; }
|
||||
/// <summary>投影保存的 TwoLeg 快照。返回世界 mm 圆形障碍物，且不访问实时检测、定位、UI 或时钟。</summary>
|
||||
public ObstacleProjectionResult ProjectToWorld() { return TwoLegObstacleProjector.Project(_input); }
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// TwoLeg 检测时刻的纯输入快照。
|
||||
///
|
||||
/// 单位：位置和两腿局部坐标均为 mm，航向为弧度。该对象不读取检测器或定位模块，确保投影可重复。
|
||||
/// </summary>
|
||||
public sealed class TwoLegProjectionInput
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建 TwoLeg 投影快照。
|
||||
///
|
||||
/// 参数：hasDetection 表示当前是否检测到目标；detectionWorldX/Y 为检测坐标系原点的世界 mm 坐标；detectionHeadingRadians 为其相对世界坐标系的航向；firstLocalX/Y、secondLocalX/Y 为两腿在检测局部坐标系中的 mm 坐标；radiusMm 为每条腿投影圆半径；diagnostic 为可选诊断文本。
|
||||
/// 注意：hasDetection 为 false 时其余几何参数不会参与有效性校验。
|
||||
/// </summary>
|
||||
public TwoLegProjectionInput(bool hasDetection, float detectionWorldX, float detectionWorldY,
|
||||
double detectionHeadingRadians, float firstLocalX, float firstLocalY,
|
||||
float secondLocalX, float secondLocalY, float radiusMm, string diagnostic = null)
|
||||
{
|
||||
HasDetection = hasDetection; DetectionWorldX = detectionWorldX; DetectionWorldY = detectionWorldY;
|
||||
DetectionHeadingRadians = detectionHeadingRadians; FirstLocalX = firstLocalX; FirstLocalY = firstLocalY;
|
||||
SecondLocalX = secondLocalX; SecondLocalY = secondLocalY; RadiusMm = radiusMm; Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
/// <summary>检测是否存在。false 时投影结果为 Empty。</summary>
|
||||
public bool HasDetection { get; }
|
||||
/// <summary>检测局部坐标系原点的世界 X 坐标，单位 mm。</summary>
|
||||
public float DetectionWorldX { get; }
|
||||
/// <summary>检测局部坐标系原点的世界 Y 坐标，单位 mm。</summary>
|
||||
public float DetectionWorldY { get; }
|
||||
/// <summary>检测局部坐标系相对世界坐标系的航向，单位弧度。</summary>
|
||||
public double DetectionHeadingRadians { get; }
|
||||
/// <summary>第一条腿在检测局部坐标系的 X 坐标，单位 mm。</summary>
|
||||
public float FirstLocalX { get; }
|
||||
/// <summary>第一条腿在检测局部坐标系的 Y 坐标，单位 mm。</summary>
|
||||
public float FirstLocalY { get; }
|
||||
/// <summary>第二条腿在检测局部坐标系的 X 坐标，单位 mm。</summary>
|
||||
public float SecondLocalX { get; }
|
||||
/// <summary>第二条腿在检测局部坐标系的 Y 坐标，单位 mm。</summary>
|
||||
public float SecondLocalY { get; }
|
||||
/// <summary>投影为圆形障碍物时的半径，单位 mm。</summary>
|
||||
public float RadiusMm { get; }
|
||||
/// <summary>上游检测阶段提供的可选诊断文本；未提供时为空字符串。</summary>
|
||||
public string Diagnostic { get; }
|
||||
/// <summary>未检测到目标时为 true；检测到目标时，所有数值有限且半径非负才为 true。</summary>
|
||||
public bool IsValid
|
||||
{
|
||||
get { return !HasDetection || (NumericGuard.IsFinite(DetectionWorldX) && NumericGuard.IsFinite(DetectionWorldY) && NumericGuard.IsFinite(DetectionHeadingRadians) && NumericGuard.IsFinite(FirstLocalX) && NumericGuard.IsFinite(FirstLocalY) && NumericGuard.IsFinite(SecondLocalX) && NumericGuard.IsFinite(SecondLocalY) && NumericGuard.IsFinite(RadiusMm) && RadiusMm >= 0f); }
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user