508 lines
23 KiB
C#
508 lines
23 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
|
|
|
/// <summary>Clumsy 粗路径手动测试可选择的固定场景。</summary>
|
|
public enum CoarsePathTestScenario
|
|
{
|
|
/// <summary>明确允许的空地图直达场景。</summary>
|
|
ExplicitEmpty,
|
|
|
|
/// <summary>由中央矩形阻断直线的绕行场景。</summary>
|
|
RectangleDetour,
|
|
|
|
/// <summary>同时包含手工圆形、矩形与 TwoLeg 快照的多来源场景。</summary>
|
|
ManualAndTwoLeg,
|
|
|
|
/// <summary>与矩形绕行输入完全一致，用于在同一服务中验证输入缓存命中。</summary>
|
|
CacheHit,
|
|
|
|
/// <summary>起步前进、终点倒车进入的换向场景。</summary>
|
|
ReverseGearSwitch,
|
|
|
|
/// <summary>由贯穿边界的障碍带分隔起终点的无解场景。</summary>
|
|
NoFeasiblePath,
|
|
}
|
|
|
|
/// <summary>手动障碍物输入支持的世界几何类型。</summary>
|
|
public enum ManualCoarsePathObstacleKind
|
|
{
|
|
/// <summary>由世界中心和半径定义的圆形障碍物。</summary>
|
|
Circle,
|
|
|
|
/// <summary>由世界中心、X 方向长度和 Y 方向宽度定义的轴对齐矩形障碍物。</summary>
|
|
AxisAlignedRectangle,
|
|
}
|
|
|
|
/// <summary>
|
|
/// 手动粗路径测试的不可变障碍物输入。
|
|
/// 所有中心和尺寸均使用世界 mm；矩形始终与世界坐标轴平行，不包含旋转角。
|
|
/// </summary>
|
|
public sealed class ManualCoarsePathObstacle
|
|
{
|
|
private ManualCoarsePathObstacle(ManualCoarsePathObstacleKind kind, double centerXMillimeters,
|
|
double centerYMillimeters, double sizeXMillimeters, double sizeYMillimeters)
|
|
{
|
|
Kind = kind;
|
|
CenterXMillimeters = centerXMillimeters;
|
|
CenterYMillimeters = centerYMillimeters;
|
|
SizeXMillimeters = sizeXMillimeters;
|
|
SizeYMillimeters = sizeYMillimeters;
|
|
}
|
|
|
|
/// <summary>障碍物的支持几何类型。</summary>
|
|
public ManualCoarsePathObstacleKind Kind { get; }
|
|
|
|
/// <summary>几何中心世界 X 坐标，单位 mm。</summary>
|
|
public double CenterXMillimeters { get; }
|
|
|
|
/// <summary>几何中心世界 Y 坐标，单位 mm。</summary>
|
|
public double CenterYMillimeters { get; }
|
|
|
|
/// <summary>圆形时为半径，矩形时为 X 方向长度；单位 mm。</summary>
|
|
public double SizeXMillimeters { get; }
|
|
|
|
/// <summary>圆形时为半径，矩形时为 Y 方向宽度；单位 mm。</summary>
|
|
public double SizeYMillimeters { get; }
|
|
|
|
/// <summary>创建圆形障碍物。参数：圆心和半径均使用世界 mm，半径必须为有限正数。</summary>
|
|
public static ManualCoarsePathObstacle Circle(double centerXMillimeters, double centerYMillimeters,
|
|
double radiusMillimeters)
|
|
{
|
|
EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
|
|
EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
|
|
EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
|
|
return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.Circle, centerXMillimeters,
|
|
centerYMillimeters, radiusMillimeters, radiusMillimeters);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 创建轴对齐矩形障碍物。
|
|
/// 参数：中心、X 方向长度和 Y 方向宽度均使用世界 mm；两个尺寸必须为有限正数。
|
|
/// </summary>
|
|
public static ManualCoarsePathObstacle AxisAlignedRectangle(double centerXMillimeters,
|
|
double centerYMillimeters, double lengthXMillimeters, double widthYMillimeters)
|
|
{
|
|
EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
|
|
EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
|
|
EnsurePositiveFinite(lengthXMillimeters, nameof(lengthXMillimeters));
|
|
EnsurePositiveFinite(widthYMillimeters, nameof(widthYMillimeters));
|
|
return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.AxisAlignedRectangle,
|
|
centerXMillimeters, centerYMillimeters, lengthXMillimeters, widthYMillimeters);
|
|
}
|
|
|
|
private static void EnsureFinite(double value, string parameterName)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value))
|
|
throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
|
|
}
|
|
|
|
private static void EnsurePositiveFinite(double value, string parameterName)
|
|
{
|
|
EnsureFinite(value, parameterName);
|
|
if (value <= 0d)
|
|
throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clumsy 粗路径测试的纯输入工厂。
|
|
/// 固定场景不读取 UI、传感器、定位或时钟；传入 AMR 位姿的手动入口仅在此处完成世界 mm/deg 到核心 m/rad 的转换。
|
|
/// </summary>
|
|
public static class CoarsePathScenarioFactory
|
|
{
|
|
private const float MapXMinMillimeters = 0f;
|
|
private const float MapXMaxMillimeters = 6000f;
|
|
private const float MapYMinMillimeters = 0f;
|
|
private const float MapYMaxMillimeters = 4000f;
|
|
private const float ResolutionMillimeters = 50f;
|
|
private const double MillimetersPerMeter = 1000d;
|
|
private const double DegreesToRadians = Math.PI / 180d;
|
|
private const double ManualMapPaddingMillimeters = 8000d;
|
|
private const int MaximumManualObstacleCount = 20;
|
|
|
|
/// <summary>
|
|
/// 创建一个新的固定测试业务请求。
|
|
/// 返回：每次调用都返回独立的可变请求对象，供调用方安全地传入同一个长期存活的规划服务。
|
|
/// </summary>
|
|
public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario)
|
|
{
|
|
return CreateCore(scenario, null);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 创建以当前 AMR 世界位姿为起点的固定测试业务请求。
|
|
/// 参数：X/Y 使用世界 mm，航向使用 deg；地图、目标和障碍物仅随 AMR 坐标平移，TwoLeg 朝向保持不变。
|
|
/// </summary>
|
|
public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario,
|
|
double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)
|
|
{
|
|
return CreateCore(scenario,
|
|
new FixedScenarioAnchor(amrXMillimeters, amrYMillimeters, amrHeadingDegrees));
|
|
}
|
|
|
|
private static CoarsePathPlanningJob CreateCore(CoarsePathTestScenario scenario, FixedScenarioAnchor anchor)
|
|
{
|
|
switch (scenario)
|
|
{
|
|
case CoarsePathTestScenario.ExplicitEmpty:
|
|
return CreateExplicitEmpty(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
|
case CoarsePathTestScenario.RectangleDetour:
|
|
case CoarsePathTestScenario.CacheHit:
|
|
return CreateRectangleDetour(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
|
case CoarsePathTestScenario.ManualAndTwoLeg:
|
|
return CreateManualAndTwoLeg(FixedScenarioTransform.From(1000d, 1000d, 0d, anchor));
|
|
case CoarsePathTestScenario.ReverseGearSwitch:
|
|
return CreateReverseGearSwitch(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
|
case CoarsePathTestScenario.NoFeasiblePath:
|
|
return CreateNoFeasiblePath(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(scenario));
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 创建传入 AMR 世界位姿和手动世界终点的空图演示请求。
|
|
/// 参数：X/Y 使用世界 mm，航向使用 deg；返回请求中的 <see cref="Pose2D"/> 使用世界 m/rad。
|
|
/// 注意：这是坐标、路径和取消流程的演示空图，不能表示现场不存在障碍物。
|
|
/// </summary>
|
|
public static CoarsePathPlanningJob CreateManualGoalDemo(
|
|
double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
|
|
double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees)
|
|
{
|
|
return CreateManualObstacleDemo(startXMillimeters, startYMillimeters, startHeadingDegrees,
|
|
goalXMillimeters, goalYMillimeters, goalHeadingDegrees,
|
|
Array.Empty<ManualCoarsePathObstacle>(), 0L);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 创建传入 AMR 世界位姿、手动世界终点和手动障碍物快照的测试请求。
|
|
/// 参数：位姿 X/Y、障碍物中心和尺寸使用世界 mm，航向使用 deg；返回的 <see cref="Pose2D"/> 使用 m/rad。
|
|
/// 障碍物非空时 obstacleSnapshotVersion 必须为正数，以避免长期服务错误复用旧地图；零障碍物才创建显式空图演示。
|
|
/// </summary>
|
|
public static CoarsePathPlanningJob CreateManualObstacleDemo(
|
|
double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
|
|
double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees,
|
|
IReadOnlyList<ManualCoarsePathObstacle> obstacles, long obstacleSnapshotVersion)
|
|
{
|
|
EnsureFinite(startXMillimeters, nameof(startXMillimeters));
|
|
EnsureFinite(startYMillimeters, nameof(startYMillimeters));
|
|
EnsureFinite(startHeadingDegrees, nameof(startHeadingDegrees));
|
|
EnsureFinite(goalXMillimeters, nameof(goalXMillimeters));
|
|
EnsureFinite(goalYMillimeters, nameof(goalYMillimeters));
|
|
EnsureFinite(goalHeadingDegrees, nameof(goalHeadingDegrees));
|
|
|
|
if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
|
|
if (obstacles.Count > MaximumManualObstacleCount)
|
|
throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle count exceeds the supported limit.");
|
|
if (obstacles.Count != 0 && obstacleSnapshotVersion <= 0L)
|
|
throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version.");
|
|
|
|
return CreateJob(
|
|
CreateManualDemoMap(startXMillimeters, startYMillimeters, goalXMillimeters, goalYMillimeters,
|
|
obstacles, obstacleSnapshotVersion),
|
|
ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
|
|
ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees),
|
|
null,
|
|
GoalDirectionConstraint.Any);
|
|
}
|
|
|
|
private static CoarsePathPlanningJob CreateExplicitEmpty(FixedScenarioTransform transform)
|
|
{
|
|
return CreateJob(
|
|
CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
|
|
transform.Pose(1d, 2d, 0d),
|
|
transform.Pose(5d, 2d, 0d),
|
|
null,
|
|
GoalDirectionConstraint.Forward);
|
|
}
|
|
|
|
private static CoarsePathPlanningJob CreateRectangleDetour(FixedScenarioTransform transform)
|
|
{
|
|
IMapObstacleSource[] sources =
|
|
{
|
|
new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
|
|
{
|
|
new AxisAlignedRectangleObstacle(transform.X(2700f), transform.X(3300f),
|
|
transform.Y(1200f), transform.Y(2800f)),
|
|
}),
|
|
};
|
|
CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
|
|
transform.Pose(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
|
|
// 固定绕行场景保留最优启发式；30 秒覆盖较慢测试环境，UI 仍可随时取消。
|
|
job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
|
|
return job;
|
|
}
|
|
|
|
private static CoarsePathPlanningJob CreateManualAndTwoLeg(FixedScenarioTransform transform)
|
|
{
|
|
IMapObstacleSource[] sources =
|
|
{
|
|
new ManualObstacleSource("manual", 2L, true, new IMapObstacle[]
|
|
{
|
|
new CircleObstacle(transform.X(2400f), transform.Y(1300f), 220f),
|
|
new AxisAlignedRectangleObstacle(transform.X(3000f), transform.X(3600f),
|
|
transform.Y(2000f), transform.Y(2600f)),
|
|
}),
|
|
new TwoLegObstacleSource("two-leg", 1L, true, new TwoLegProjectionInput(true,
|
|
transform.X(3900f), transform.Y(2500f), 0d,
|
|
-180f, -180f, -180f, 180f, 140f, "P1 fixed TwoLeg snapshot.")),
|
|
};
|
|
CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 1d, 0d),
|
|
transform.Pose(5d, 3d, 0d), null, GoalDirectionConstraint.Forward);
|
|
// 多来源场景的最优绕行会受机器负载影响；放宽演示总预算但保留全部碰撞与目标判定。
|
|
job.Configuration.SearchTimeout = TimeSpan.FromSeconds(15d);
|
|
return job;
|
|
}
|
|
|
|
private static CoarsePathPlanningJob CreateReverseGearSwitch(FixedScenarioTransform transform)
|
|
{
|
|
return CreateJob(
|
|
CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
|
|
transform.Pose(1d, 2d, 0d),
|
|
transform.Pose(4d, 2d, 0d),
|
|
TravelDirection.Forward,
|
|
GoalDirectionConstraint.Reverse);
|
|
}
|
|
|
|
private static CoarsePathPlanningJob CreateNoFeasiblePath(FixedScenarioTransform transform)
|
|
{
|
|
IMapObstacleSource[] sources =
|
|
{
|
|
new ManualObstacleSource("manual", 3L, true, new IMapObstacle[]
|
|
{
|
|
new AxisAlignedRectangleObstacle(transform.X(2900f), transform.X(3100f),
|
|
transform.Y(0f), transform.Y(4000f)),
|
|
}),
|
|
};
|
|
return CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
|
|
transform.Pose(5d, 2d, 0d),
|
|
null, GoalDirectionConstraint.Forward);
|
|
}
|
|
|
|
private static CoarsePathPlanningJob CreateJob(PlanningMapRequest mapRequest, Pose2D start, Pose2D goal,
|
|
TravelDirection? startDirection, GoalDirectionConstraint goalDirection)
|
|
{
|
|
return new CoarsePathPlanningJob
|
|
{
|
|
MapRequest = mapRequest,
|
|
Start = start,
|
|
Goal = goal,
|
|
Vehicle = new VehicleParameters
|
|
{
|
|
LengthMeters = 0.80d,
|
|
WidthMeters = 0.60d,
|
|
SafetyMarginMeters = 0.05d,
|
|
MaximumCurvaturePerMeter = 1d / 1.20d,
|
|
},
|
|
Configuration = new HybridAStarConfiguration(),
|
|
StartDirection = startDirection,
|
|
GoalDirection = goalDirection,
|
|
};
|
|
}
|
|
|
|
private static PlanningMapRequest CreateMap(bool allowExplicitEmptyMap, IReadOnlyList<IMapObstacleSource> sources,
|
|
FixedScenarioTransform transform)
|
|
{
|
|
return new PlanningMapRequest
|
|
{
|
|
Bounds = new MapBoundsMm(transform.X(MapXMinMillimeters), transform.X(MapXMaxMillimeters),
|
|
transform.Y(MapYMinMillimeters), transform.Y(MapYMaxMillimeters)),
|
|
ResolutionMm = ResolutionMillimeters,
|
|
ObstacleSources = sources,
|
|
AllowExplicitEmptyMap = allowExplicitEmptyMap,
|
|
};
|
|
}
|
|
|
|
private sealed class FixedScenarioAnchor
|
|
{
|
|
public FixedScenarioAnchor(double xMillimeters, double yMillimeters, double headingDegrees)
|
|
{
|
|
EnsureFinite(xMillimeters, nameof(xMillimeters));
|
|
EnsureFinite(yMillimeters, nameof(yMillimeters));
|
|
EnsureFinite(headingDegrees, nameof(headingDegrees));
|
|
XMillimeters = xMillimeters;
|
|
YMillimeters = yMillimeters;
|
|
HeadingRadians = NormalizeRadians((headingDegrees % 360d) * DegreesToRadians);
|
|
}
|
|
|
|
public double XMillimeters { get; }
|
|
public double YMillimeters { get; }
|
|
public double HeadingRadians { get; }
|
|
}
|
|
|
|
private sealed class FixedScenarioTransform
|
|
{
|
|
private FixedScenarioTransform(double deltaXMillimeters, double deltaYMillimeters, double headingDeltaRadians)
|
|
{
|
|
DeltaXMillimeters = deltaXMillimeters;
|
|
DeltaYMillimeters = deltaYMillimeters;
|
|
HeadingDeltaRadians = headingDeltaRadians;
|
|
}
|
|
|
|
public double DeltaXMillimeters { get; }
|
|
public double DeltaYMillimeters { get; }
|
|
public double HeadingDeltaRadians { get; }
|
|
|
|
public static FixedScenarioTransform From(double baselineStartXMillimeters,
|
|
double baselineStartYMillimeters, double baselineStartHeadingRadians, FixedScenarioAnchor anchor)
|
|
{
|
|
if (anchor == null) return new FixedScenarioTransform(0d, 0d, 0d);
|
|
return new FixedScenarioTransform(anchor.XMillimeters - baselineStartXMillimeters,
|
|
anchor.YMillimeters - baselineStartYMillimeters,
|
|
NormalizeRadians(anchor.HeadingRadians - baselineStartHeadingRadians));
|
|
}
|
|
|
|
public float X(float value)
|
|
{
|
|
return ToFiniteFloat(value + DeltaXMillimeters, nameof(value));
|
|
}
|
|
|
|
public float Y(float value)
|
|
{
|
|
return ToFiniteFloat(value + DeltaYMillimeters, nameof(value));
|
|
}
|
|
|
|
public Pose2D Pose(double xMeters, double yMeters, double headingRadians)
|
|
{
|
|
return new Pose2D((xMeters * MillimetersPerMeter + DeltaXMillimeters) / MillimetersPerMeter,
|
|
(yMeters * MillimetersPerMeter + DeltaYMillimeters) / MillimetersPerMeter,
|
|
NormalizeRadians(headingRadians + HeadingDeltaRadians));
|
|
}
|
|
}
|
|
|
|
private static PlanningMapRequest CreateManualDemoMap(double startXMillimeters, double startYMillimeters,
|
|
double goalXMillimeters, double goalYMillimeters, IReadOnlyList<ManualCoarsePathObstacle> obstacles,
|
|
long obstacleSnapshotVersion)
|
|
{
|
|
double minimumX = Math.Min(startXMillimeters, goalXMillimeters);
|
|
double maximumX = Math.Max(startXMillimeters, goalXMillimeters);
|
|
double minimumY = Math.Min(startYMillimeters, goalYMillimeters);
|
|
double maximumY = Math.Max(startYMillimeters, goalYMillimeters);
|
|
for (int index = 0; index < obstacles.Count; index++)
|
|
{
|
|
ManualCoarsePathObstacle obstacle = obstacles[index] ??
|
|
throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
|
|
double halfX;
|
|
double halfY;
|
|
switch (obstacle.Kind)
|
|
{
|
|
case ManualCoarsePathObstacleKind.Circle:
|
|
EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
|
|
halfX = obstacle.SizeXMillimeters;
|
|
halfY = obstacle.SizeYMillimeters;
|
|
break;
|
|
case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
|
|
EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
|
|
EnsurePositiveFinite(obstacle.SizeYMillimeters, nameof(obstacles));
|
|
halfX = obstacle.SizeXMillimeters / 2d;
|
|
halfY = obstacle.SizeYMillimeters / 2d;
|
|
break;
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
|
|
}
|
|
|
|
minimumX = Math.Min(minimumX, obstacle.CenterXMillimeters - halfX);
|
|
maximumX = Math.Max(maximumX, obstacle.CenterXMillimeters + halfX);
|
|
minimumY = Math.Min(minimumY, obstacle.CenterYMillimeters - halfY);
|
|
maximumY = Math.Max(maximumY, obstacle.CenterYMillimeters + halfY);
|
|
}
|
|
|
|
float xMin = ToGridLowerBound(minimumX - ManualMapPaddingMillimeters);
|
|
float xMax = ToGridUpperBound(maximumX + ManualMapPaddingMillimeters);
|
|
float yMin = ToGridLowerBound(minimumY - ManualMapPaddingMillimeters);
|
|
float yMax = ToGridUpperBound(maximumY + ManualMapPaddingMillimeters);
|
|
bool isExplicitEmptyMap = obstacles.Count == 0;
|
|
return new PlanningMapRequest
|
|
{
|
|
Bounds = new MapBoundsMm(xMin, xMax, yMin, yMax),
|
|
ResolutionMm = ResolutionMillimeters,
|
|
ObstacleSources = isExplicitEmptyMap ? Array.Empty<IMapObstacleSource>() :
|
|
new IMapObstacleSource[]
|
|
{
|
|
new ManualObstacleSource("manual-user-input", obstacleSnapshotVersion, true,
|
|
ConvertManualObstacles(obstacles)),
|
|
},
|
|
AllowExplicitEmptyMap = isExplicitEmptyMap,
|
|
};
|
|
}
|
|
|
|
private static IMapObstacle[] ConvertManualObstacles(IReadOnlyList<ManualCoarsePathObstacle> obstacles)
|
|
{
|
|
var result = new IMapObstacle[obstacles.Count];
|
|
for (int index = 0; index < obstacles.Count; index++)
|
|
{
|
|
ManualCoarsePathObstacle obstacle = obstacles[index] ??
|
|
throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
|
|
switch (obstacle.Kind)
|
|
{
|
|
case ManualCoarsePathObstacleKind.Circle:
|
|
result[index] = new CircleObstacle(ToFiniteFloat(obstacle.CenterXMillimeters, nameof(obstacles)),
|
|
ToFiniteFloat(obstacle.CenterYMillimeters, nameof(obstacles)),
|
|
ToFiniteFloat(obstacle.SizeXMillimeters, nameof(obstacles)));
|
|
break;
|
|
case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
|
|
double halfX = obstacle.SizeXMillimeters / 2d;
|
|
double halfY = obstacle.SizeYMillimeters / 2d;
|
|
result[index] = new AxisAlignedRectangleObstacle(
|
|
ToFiniteFloat(obstacle.CenterXMillimeters - halfX, nameof(obstacles)),
|
|
ToFiniteFloat(obstacle.CenterXMillimeters + halfX, nameof(obstacles)),
|
|
ToFiniteFloat(obstacle.CenterYMillimeters - halfY, nameof(obstacles)),
|
|
ToFiniteFloat(obstacle.CenterYMillimeters + halfY, nameof(obstacles)));
|
|
break;
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
private static Pose2D ToPose(double xMillimeters, double yMillimeters, double headingDegrees)
|
|
{
|
|
return new Pose2D(xMillimeters / MillimetersPerMeter, yMillimeters / MillimetersPerMeter,
|
|
headingDegrees * DegreesToRadians);
|
|
}
|
|
|
|
private static double NormalizeRadians(double angle)
|
|
{
|
|
double normalized = angle % (2d * Math.PI);
|
|
if (normalized <= -Math.PI) return normalized + 2d * Math.PI;
|
|
return normalized > Math.PI ? normalized - 2d * Math.PI : normalized;
|
|
}
|
|
|
|
private static float ToGridLowerBound(double millimeters)
|
|
{
|
|
double rounded = Math.Floor(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
|
|
return ToFiniteFloat(rounded, nameof(millimeters));
|
|
}
|
|
|
|
private static float ToGridUpperBound(double millimeters)
|
|
{
|
|
double rounded = Math.Ceiling(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
|
|
return ToFiniteFloat(rounded, nameof(millimeters));
|
|
}
|
|
|
|
private static float ToFiniteFloat(double value, string parameterName)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
|
|
throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate.");
|
|
return (float)value;
|
|
}
|
|
|
|
private static void EnsureFinite(double value, string parameterName)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value))
|
|
throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
|
|
}
|
|
|
|
private static void EnsurePositiveFinite(double value, string parameterName)
|
|
{
|
|
EnsureFinite(value, parameterName);
|
|
if (value <= 0d)
|
|
throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
|
|
}
|
|
}
|