312 lines
18 KiB
C#
312 lines
18 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
using Trajectory2D = MultiWheelC.Trajectory.Trajectory2D;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.Guide;
|
|
|
|
/// <summary>固定离线示例的只读输出，同时保留 EM 时间轨迹和控制器几何轨迹。</summary>
|
|
public sealed class FixedOfflinePlanningOutput
|
|
{
|
|
public FixedOfflinePlanningOutput(
|
|
EmTrajectory emTrajectory, // 来源：EM 发布闸门后的非空时间轨迹。
|
|
Trajectory2D controllerTrajectory) // 来源：同一 EM 轨迹经控制适配器生成的 SI 轨迹。
|
|
{
|
|
EmTrajectory = emTrajectory ??
|
|
throw new ArgumentNullException(nameof(emTrajectory));
|
|
ControllerTrajectory = controllerTrajectory ??
|
|
throw new ArgumentNullException(nameof(controllerTrajectory));
|
|
}
|
|
|
|
public EmTrajectory EmTrajectory { get; }
|
|
|
|
public Trajectory2D ControllerTrajectory { get; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// 展示固定、静态、单方向的离线规划数据流；本类不读取实时定位，也不发送底盘命令。
|
|
/// </summary>
|
|
public static class EmPlannerFullPipelineDemo
|
|
{
|
|
public static FixedOfflinePlanningOutput CreateFixedOfflineTrajectory(
|
|
CancellationToken cancellationToken)
|
|
{
|
|
// 步骤 1：定义固定世界起点和终点
|
|
var startPose = new Pose2D(
|
|
1.50d, // 来源：固定场景起点世界 X；单位 m。
|
|
1.50d, // 来源：固定场景起点世界 Y；单位 m。
|
|
0d); // 来源：固定场景起点车头航向；单位 rad。
|
|
var goalPose = new Pose2D(
|
|
6.50d, // 来源：固定场景终点世界 X；单位 m。
|
|
1.50d, // 来源：固定场景终点世界 Y；单位 m。
|
|
0d); // 来源：固定场景终点车头航向；单位 rad。
|
|
|
|
// 步骤 2：定义车辆、曲率和搜索预算
|
|
const double vehicleLengthMeters = 1.00d;
|
|
const double vehicleWidthMeters = 0.60d;
|
|
const double safetyMarginMeters = 0.10d;
|
|
const double maximumCurvaturePerMeter = 0.80d;
|
|
var vehicle = new VehicleParameters
|
|
{
|
|
LengthMeters = vehicleLengthMeters, // 来源：固定车辆本体长度；单位 m。
|
|
WidthMeters = vehicleWidthMeters, // 来源：固定车辆本体宽度；单位 m。
|
|
SafetyMarginMeters = safetyMarginMeters, // 来源：固定碰撞安全余量；单位 m。
|
|
MaximumCurvaturePerMeter = maximumCurvaturePerMeter, // 来源：固定运动学上限；单位 1/m。
|
|
MinimumTurningRadiusMeters = null, // 来源：本示例已直接给出最大曲率；单位 m。
|
|
};
|
|
var coarseConfiguration = new HybridAStarConfiguration
|
|
{
|
|
PrimitiveLengthMeters = 0.40d, // 来源：固定搜索原语长度；单位 m。
|
|
IntegrationStepMeters = 0.05d, // 来源：固定原语积分间距；单位 m。
|
|
MaximumCollisionCheckStepMeters = 0.025d, // 来源：固定连续碰撞检查步长；单位 m。
|
|
HeadingResolutionRadians = Math.PI / 36d, // 来源：固定航向离散；单位 rad。
|
|
CurvatureLevelCount = 5, // 来源：固定曲率离散级数；无单位。
|
|
GoalPositionToleranceMeters = 0.15d, // 来源：固定终点位置容差；单位 m。
|
|
GoalHeadingToleranceRadians = Math.PI / 36d, // 来源：固定终点航向容差；单位 rad。
|
|
MaximumExpandedNodes = 100000, // 来源：固定节点预算；单位为节点数。
|
|
SearchTimeout = TimeSpan.FromSeconds(5d), // 来源：固定搜索时间预算；单位 s。
|
|
HeuristicWeight = 1d, // 来源：固定启发式权重；无单位。
|
|
ReverseCostMultiplier = 1.5d, // 来源：固定倒车代价倍率；无单位。
|
|
GearSwitchPenaltyMeters = 1d, // 来源：固定换向惩罚；单位 m 等价代价。
|
|
CurvatureMagnitudeWeight = 0.10d, // 来源：固定曲率代价权重；无单位。
|
|
CurvatureChangePenaltyMetersPerLevel = 0.05d, // 来源：固定曲率变化惩罚；单位 m/级。
|
|
ClearanceCostWeight = 0.20d, // 来源：固定净空代价权重；无单位。
|
|
ClearanceCostDistanceMeters = 0.50d, // 来源：固定净空代价作用距离；单位 m。
|
|
AllowReverse = false, // 来源：固定场景只允许前进；无单位。
|
|
};
|
|
|
|
// 步骤 3：建立静态地图
|
|
var bounds = new MapBoundsMm(
|
|
0f, // 来源：固定地图 X 下界；单位 mm。
|
|
8000f, // 来源：固定地图 X 上界；单位 mm。
|
|
0f, // 来源：固定地图 Y 下界；单位 mm。
|
|
5000f); // 来源：固定地图 Y 上界；单位 mm。
|
|
const float resolutionMm = 50f;
|
|
IReadOnlyList<IMapObstacle> obstacles = new IMapObstacle[]
|
|
{
|
|
new CircleObstacle(
|
|
4000f, // 来源：固定圆障碍圆心 X；单位 mm。
|
|
3500f, // 来源：固定圆障碍圆心 Y；单位 mm。
|
|
400f), // 来源：固定圆障碍半径；单位 mm。
|
|
};
|
|
var obstacleSource = new ManualObstacleSource(
|
|
"fixed-offline-obstacles", // 来源：固定障碍快照身份；无单位。
|
|
1L, // 来源：固定障碍快照版本；非负版本号。
|
|
true, // 来源：障碍来源必须成功投影；无单位。
|
|
obstacles); // 来源：上方固定世界障碍列表；几何坐标单位 mm。
|
|
var mapRequest = new PlanningMapRequest
|
|
{
|
|
Bounds = bounds, // 来源：固定地图边界；单位 mm。
|
|
ResolutionMm = resolutionMm, // 来源：固定栅格边长；单位 mm。
|
|
ObstacleSources = new IMapObstacleSource[] { obstacleSource }, // 来源：固定障碍来源快照。
|
|
AllowExplicitEmptyMap = false, // 来源：本场景要求障碍物真实参与建图；无单位。
|
|
};
|
|
PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(mapRequest);
|
|
if (!mapResult.Succeeded || mapResult.Map == null || !mapResult.Map.PlanningReady)
|
|
{
|
|
string reason = mapResult.Map != null && !mapResult.Map.PlanningReady
|
|
? mapResult.Map.PlanningBlockReason
|
|
: mapResult.FailureReason;
|
|
throw new InvalidOperationException("固定离线地图不可用于规划：" + reason);
|
|
}
|
|
PlanningGridMap map = mapResult.Map;
|
|
|
|
// 步骤 4：建立粗路径请求
|
|
var coarseRequest = new PlanningRequest
|
|
{
|
|
Map = map, // 来源：步骤 3 的冻结地图快照。
|
|
Start = startPose, // 来源：步骤 1 的起点；位置 m、航向 rad。
|
|
Goal = goalPose, // 来源：步骤 1 的终点；位置 m、航向 rad。
|
|
Vehicle = vehicle, // 来源：步骤 2 的车辆；尺寸 m、曲率 1/m。
|
|
Configuration = coarseConfiguration, // 来源：步骤 2 的固定搜索配置。
|
|
StartVehicleCurvature = 0d, // 来源：固定起步直行状态；单位 1/m。
|
|
StartDirection = TravelDirection.Forward, // 来源：固定场景仅前进；无单位。
|
|
GoalDirection = GoalDirectionConstraint.Forward, // 来源：固定终点以前进方向到达；无单位。
|
|
};
|
|
|
|
// 步骤 5：搜索粗路径
|
|
PlanningResult coarseResult = new HybridAStarPlanner().Plan(
|
|
coarseRequest,
|
|
cancellationToken);
|
|
if (coarseResult.Status != PlanningStatus.Success)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"Hybrid A* 粗路径失败：" +
|
|
coarseResult.Diagnostics.TerminationReason);
|
|
}
|
|
|
|
// 步骤 6：请求并执行 Local G2 平滑
|
|
var smoothingConfiguration = new PathSmoothingConfiguration
|
|
{
|
|
OutputSpacingMeters = 0.025d, // 来源：固定平滑输出间距；单位 m。
|
|
MaximumCollisionCheckStepMeters = 0.025d, // 来源：固定扫掠复核步长；单位 m。
|
|
MinimumClearanceReserveMeters = 0.02d, // 来源：固定额外净空；单位 m。
|
|
CurvatureLimitRadiusToleranceMeters = 0.002d, // 来源：固定曲率半径容差；单位 m。
|
|
};
|
|
smoothingConfiguration.LocalG2Quintic.MinimumWindowLengthMeters = 0.20d; // 单位 m。
|
|
smoothingConfiguration.LocalG2Quintic.PreferredWindowLengthMeters = 0.50d; // 单位 m。
|
|
smoothingConfiguration.LocalG2Quintic.MaximumWindowLengthMeters = 0.80d; // 单位 m。
|
|
smoothingConfiguration.LocalG2Quintic.MaximumDeviationMeters = 0.10d; // 单位 m。
|
|
smoothingConfiguration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = 0.001d; // 单位 1/m。
|
|
smoothingConfiguration.LocalG2Quintic.CurvatureJumpRatioOfMaximum = 0.05d; // 无单位。
|
|
smoothingConfiguration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.20d; // 无单位。
|
|
smoothingConfiguration.LocalG2Quintic.MaximumVariationCostRegressionRatio = 0.02d; // 无单位。
|
|
smoothingConfiguration.LocalG2Quintic.MaximumCandidatesPerRegion = 12; // 单位为候选数。
|
|
var smoothingRequest = new PathSmoothingRequest(
|
|
coarseResult.Path, // 来源：成功粗路径；位置与弧长单位 m、航向 rad。
|
|
coarseResult.Segments, // 来源：成功粗路径的方向分段；无单位。
|
|
map, // 来源：步骤 3 的同一冻结地图快照。
|
|
vehicle, // 来源：步骤 2 的同一车辆约束；尺寸 m、曲率 1/m。
|
|
smoothingConfiguration); // 来源：本步骤显式固定的 Local G2 配置。
|
|
PathSmoothingResult smoothingResult = new PathSmoothingService().Smooth(
|
|
smoothingRequest,
|
|
cancellationToken);
|
|
bool smoothingSucceeded =
|
|
smoothingResult.Status == PathSmoothingStatus.Complete ||
|
|
smoothingResult.Status == PathSmoothingStatus.PartialImprovement ||
|
|
smoothingResult.Status == PathSmoothingStatus.NotNeeded ||
|
|
smoothingResult.Status == PathSmoothingStatus.Unchanged;
|
|
if (!smoothingSucceeded ||
|
|
smoothingResult.Path == null || smoothingResult.Path.Count == 0 ||
|
|
smoothingResult.Segments == null || smoothingResult.Segments.Count == 0)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"Local G2 平滑没有发布完整路径：" +
|
|
smoothingResult.Diagnostics.TerminationReason);
|
|
}
|
|
|
|
// 步骤 7：选择一个方向段
|
|
IReadOnlyList<DirectionSegmentView> directionSegments =
|
|
ReferencePathSegmenter.Create(smoothingResult);
|
|
if (directionSegments.Count != 1)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"固定控制器演示不得跨越换挡边界，平滑路径必须恰好包含一个方向段。");
|
|
}
|
|
DirectionSegmentView activeSegment = directionSegments[0];
|
|
if (activeSegment.Direction != TravelDirection.Forward)
|
|
{
|
|
throw new InvalidOperationException("固定离线演示只接受前进方向段。");
|
|
}
|
|
|
|
// 步骤 8：建立初始运动状态
|
|
DateTimeOffset planningTimeUtc = DateTimeOffset.UtcNow;
|
|
var vehicleState = new VehicleMotionState(
|
|
startPose, // 来源：步骤 1 的固定起点；位置 m、航向 rad。
|
|
0d, // 来源：固定静止起步状态；有符号纵向速度单位 m/s。
|
|
0d, // 来源：固定静止起步状态；纵向加速度单位 m/s²。
|
|
planningTimeUtc, // 来源：本次离线调用的 UTC 快照时刻。
|
|
0L); // 来源：固定离线状态版本；非负序列号。
|
|
|
|
// 步骤 9：冻结 EM 配置
|
|
EmPlannerConfiguration emConfiguration = EmPlannerConfiguration.CreateDefault();
|
|
emConfiguration.Scheduling.TimeHorizonSeconds = 20d; // 固定完整段时间窗口；单位 s。
|
|
emConfiguration.Scheduling.DistanceHorizonMeters = 10d; // 固定空间窗口；单位 m。
|
|
emConfiguration.Scheduling.OutputTimeStepSeconds = 0.10d; // 固定发布间隔；单位 s。
|
|
emConfiguration.Scheduling.SolverTimeoutSeconds = 5d; // 固定求解预算；单位 s。
|
|
emConfiguration.Scheduling.MaximumVehicleStateAgeSeconds = 1d; // 固定状态时效；单位 s。
|
|
emConfiguration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.60d; // 单位 m/s。
|
|
emConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.40d; // 单位 m/s。
|
|
emConfiguration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 0.40d; // 单位 m/s²。
|
|
emConfiguration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 0.40d; // 单位 m/s²。
|
|
emConfiguration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1.00d; // 单位 m/s³。
|
|
emConfiguration.Solver.MaximumOuterIterations = 5; // 固定顺序凸化迭代数。
|
|
emConfiguration.Solver.MaximumOsqpIterations = 10000; // 固定 OSQP 迭代数。
|
|
|
|
// 步骤 10：建立 EM 请求
|
|
var emRequest = new EmPlanningRequest(
|
|
smoothingResult, // 来源：步骤 6 的完整平滑路径；位置与弧长单位 m。
|
|
map, // 来源：步骤 3 的冻结地图快照。
|
|
vehicle, // 来源：步骤 2 的车辆约束；尺寸 m、曲率 1/m。
|
|
vehicleState, // 来源：步骤 8 的固定运动状态；SI 单位、UTC。
|
|
emConfiguration, // 来源：步骤 9 的显式低速泊车配置。
|
|
0, // 来源：步骤 7 已验证的唯一方向段；零基索引。
|
|
null, // 来源：首次固定离线规划没有上一条轨迹。
|
|
planningTimeUtc, // 来源：本次请求发起 UTC 时刻。
|
|
planningTimeUtc, // 来源：本次轨迹生效 UTC 时刻。
|
|
"fixed-offline-em-trajectory", // 来源：固定非空输出轨迹身份。
|
|
"fixed-offline-reference-path", // 来源：固定非空参考路径身份。
|
|
string.Empty, // 来源：没有上一条轨迹，因此没有前序身份。
|
|
EmMotionModel.NonholonomicForwardReverse, // 来源：当前 EM 支持的非完整运动模型。
|
|
EmPlanningScope.FullDirectionSegment, // 来源：固定示例规划完整单方向段。
|
|
null, // 来源：离线示例不另设循环截止剩余时间；单位 s。
|
|
cancellationToken, // 来源：方法调用方的取消令牌。
|
|
CancellationToken.None); // 来源：离线示例没有独立循环截止令牌。
|
|
|
|
// 步骤 11：调用 EM 服务
|
|
var emPlanningService = new EmPlanningService(
|
|
new OsqpNativeSolver()); // 来源：仓库现有原生 OSQP 求解器；无单位。
|
|
EmPlanningResult emResult = emPlanningService.Plan(
|
|
emRequest,
|
|
cancellationToken);
|
|
|
|
// 步骤 12：判断是否可发布
|
|
bool emSucceeded = emResult.Status == EmPlanningStatus.Success ||
|
|
emResult.Status == EmPlanningStatus.SuccessWithFallback;
|
|
if (!emSucceeded ||
|
|
emResult.Trajectory == null ||
|
|
emResult.Trajectory.Points.Count < 2)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"EM 规划没有返回完整可控轨迹：" + emResult.FailureReason);
|
|
}
|
|
EmTrajectory emTrajectory = emResult.Trajectory;
|
|
|
|
// 步骤 13：读取输出契约
|
|
// 以下局部变量只用于逐字段检查与诊断示范，不是车辆命令，也绝不能直接下发到底盘。
|
|
string trajectoryId = emTrajectory.Metadata.TrajectoryId;
|
|
string referencePathId = emTrajectory.Metadata.ReferencePathId;
|
|
TravelDirection publishedDirection = emTrajectory.Metadata.Direction;
|
|
EmTerminalType terminalType = emTrajectory.Metadata.TerminalType;
|
|
EmPlanningScope publishedScope = emTrajectory.Metadata.PlanningScope;
|
|
_ = trajectoryId;
|
|
_ = referencePathId;
|
|
_ = publishedDirection;
|
|
_ = terminalType;
|
|
_ = publishedScope;
|
|
foreach (EmTrajectoryPoint point in emTrajectory.Points)
|
|
{
|
|
double timeFromStartSeconds = point.TimeFromStart;
|
|
double worldXMeters = point.X;
|
|
double worldYMeters = point.Y;
|
|
double yawRadians = point.Yaw;
|
|
double vehicleCurvaturePerMeter = point.VehicleCurvature;
|
|
double pathSMeters = point.PathS;
|
|
double signedSpeedMetersPerSecond = point.SignedLongitudinalVelocity;
|
|
double velocityXMetersPerSecond = point.VelocityX;
|
|
double velocityYMetersPerSecond = point.VelocityY;
|
|
double longitudinalAccelerationMetersPerSecondSquared = point.LongitudinalAcceleration;
|
|
double longitudinalJerkMetersPerSecondCubed = point.LongitudinalJerk;
|
|
double yawRateRadiansPerSecond = point.YawRate;
|
|
_ = timeFromStartSeconds;
|
|
_ = worldXMeters;
|
|
_ = worldYMeters;
|
|
_ = yawRadians;
|
|
_ = vehicleCurvaturePerMeter;
|
|
_ = pathSMeters;
|
|
_ = signedSpeedMetersPerSecond;
|
|
_ = velocityXMetersPerSecond;
|
|
_ = velocityYMetersPerSecond;
|
|
_ = longitudinalAccelerationMetersPerSecondSquared;
|
|
_ = longitudinalJerkMetersPerSecondCubed;
|
|
_ = yawRateRadiansPerSecond;
|
|
}
|
|
|
|
// 步骤 14：适配为控制轨迹
|
|
Trajectory2D controllerTrajectory =
|
|
new EmControlTrajectoryAdapter().Create(emTrajectory);
|
|
|
|
// 步骤 15：交给闭环动作
|
|
// 本示例只返回轨迹输入；StateProvider、控制器参数、执行保护和停车策略仍由宿主拥有。
|
|
return new FixedOfflinePlanningOutput(
|
|
emTrajectory, // 来源：步骤 12 的完整 EM 时间轨迹。
|
|
controllerTrajectory); // 来源：步骤 14 的控制器 SI 轨迹。
|
|
}
|
|
}
|