33 lines
1.8 KiB
C#
33 lines
1.8 KiB
C#
|
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);
|
||
|
|
}
|
||
|
|
}
|