174 lines
8.7 KiB
C#
174 lines
8.7 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||
|
||
/// <summary>
|
||
/// 生成经解析积分和连续碰撞检查的前进或倒车恒曲率原语。
|
||
/// 每个积分点均依次经过有限数值检查、与前一点之间的扫掠碰撞检查和终点容差检查。
|
||
/// </summary>
|
||
public sealed class MotionPrimitiveGenerator
|
||
{
|
||
private const double StraightCurvatureThreshold = 1e-12d;
|
||
private readonly FootprintCollisionChecker _collisionChecker;
|
||
|
||
/// <summary>创建使用默认车辆连续碰撞检查器的原语生成器。</summary>
|
||
public MotionPrimitiveGenerator()
|
||
: this(new FootprintCollisionChecker())
|
||
{
|
||
}
|
||
|
||
/// <summary>创建使用指定连续车辆碰撞检查器的原语生成器。</summary>
|
||
public MotionPrimitiveGenerator(FootprintCollisionChecker collisionChecker)
|
||
{
|
||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用完整规划请求生成一条原语。
|
||
/// 参数：start 为当前连续位姿；curvaturePerMeter 为候选恒定曲率；direction 为前进或倒车；request 提供地图、车辆、配置和目标。
|
||
/// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；否则返回最大长度不超过配置上限的原语。
|
||
/// </summary>
|
||
public MotionPrimitive Generate(Pose2D start, double curvaturePerMeter, TravelDirection direction, PlanningRequest request)
|
||
{
|
||
if (request == null) return null;
|
||
return Generate(start, curvaturePerMeter, direction, request.Map, request.Vehicle, request.Configuration, request.Goal, request.GoalDirection);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用显式地图、车辆、配置和目标生成一条原语。
|
||
/// 参数：所有位置使用 m/rad，curvaturePerMeter 使用 1/m；goalDirection 限制末段允许的进入方向。
|
||
/// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；首次命中目标时返回 <see cref="MotionPrimitive.IsGoalTruncation"/> 为 true 的截断原语。
|
||
/// </summary>
|
||
public MotionPrimitive Generate(
|
||
Pose2D start,
|
||
double curvaturePerMeter,
|
||
TravelDirection direction,
|
||
PlanningGridMap map,
|
||
VehicleParameters vehicle,
|
||
HybridAStarConfiguration configuration,
|
||
Pose2D goal,
|
||
GoalDirectionConstraint goalDirection)
|
||
{
|
||
if (!IsValidInput(start, curvaturePerMeter, direction, map, vehicle, configuration, goal, goalDirection)) return null;
|
||
|
||
if (GoalToleranceChecker.IsSatisfied(start, goal, configuration, direction, goalDirection))
|
||
return new MotionPrimitive(start, direction, curvaturePerMeter, 0d, Array.Empty<Pose2D>(), Array.Empty<double>(), true);
|
||
|
||
double pointStepMeters = Math.Min(configuration.IntegrationStepMeters,
|
||
Math.Min(configuration.MaximumCollisionCheckStepMeters, map.ResolutionMeters / 2d));
|
||
if (!NumericGuard.IsPositiveFinite(pointStepMeters)) return null;
|
||
|
||
var points = new List<Pose2D>();
|
||
var bodyClearancesMeters = new List<double>();
|
||
Pose2D previous = start;
|
||
double actualLengthMeters = 0d;
|
||
while (actualLengthMeters < configuration.PrimitiveLengthMeters)
|
||
{
|
||
double remainingLengthMeters = configuration.PrimitiveLengthMeters - actualLengthMeters;
|
||
double stepMeters = Math.Min(pointStepMeters, remainingLengthMeters);
|
||
if (!NumericGuard.IsPositiveFinite(stepMeters)) return null;
|
||
|
||
Pose2D next = Integrate(previous, curvaturePerMeter, direction, stepMeters);
|
||
if (!IsFinitePose(next)) return null;
|
||
if (!_collisionChecker.IsSweptMotionCollisionFree(previous, next, map, vehicle, stepMeters, out double bodyClearanceMeters)) return null;
|
||
|
||
actualLengthMeters += stepMeters;
|
||
points.Add(next);
|
||
bodyClearancesMeters.Add(bodyClearanceMeters);
|
||
if (GoalToleranceChecker.IsSatisfied(next, goal, configuration, direction, goalDirection))
|
||
return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, true);
|
||
|
||
previous = next;
|
||
}
|
||
|
||
return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, false);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取从最大负曲率到最大正曲率均匀分布的候选曲率等级。
|
||
/// 参数：vehicle 提供保守最大曲率；configuration 的曲率等级数必须为不小于 3 的奇数。
|
||
/// 返回：输入无效时为空只读列表；有效时长度等于配置等级数且中间等级恒为零曲率。
|
||
/// </summary>
|
||
public IReadOnlyList<double> GetCurvatureLevels(VehicleParameters vehicle, HybridAStarConfiguration configuration)
|
||
{
|
||
if (configuration == null || configuration.CurvatureLevelCount < 3 || configuration.CurvatureLevelCount % 2 == 0 ||
|
||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||
return Array.Empty<double>();
|
||
|
||
var levels = new double[configuration.CurvatureLevelCount];
|
||
double increment = 2d * maximumCurvaturePerMeter / (levels.Length - 1d);
|
||
for (int index = 0; index < levels.Length; index++) levels[index] = -maximumCurvaturePerMeter + increment * index;
|
||
levels[levels.Length / 2] = 0d;
|
||
return levels;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断两个曲率等级是否允许在相邻原语间直接切换。
|
||
/// 参数：previousLevelIndex 与 nextLevelIndex 为从零开始的等级索引。
|
||
/// 返回：两个索引均非负且最多相差一个等级时为 true。
|
||
/// </summary>
|
||
public static bool AreCurvatureLevelsAdjacent(int previousLevelIndex, int nextLevelIndex)
|
||
{
|
||
return previousLevelIndex >= 0 && nextLevelIndex >= 0 && Math.Abs(previousLevelIndex - nextLevelIndex) <= 1;
|
||
}
|
||
|
||
private static bool IsValidInput(
|
||
Pose2D start,
|
||
double curvaturePerMeter,
|
||
TravelDirection direction,
|
||
PlanningGridMap map,
|
||
VehicleParameters vehicle,
|
||
HybridAStarConfiguration configuration,
|
||
Pose2D goal,
|
||
GoalDirectionConstraint goalDirection)
|
||
{
|
||
if (!IsFinitePose(start) || !IsFinitePose(goal) || map == null || vehicle == null || configuration == null ||
|
||
!NumericGuard.IsFinite(curvaturePerMeter) || !IsTravelDirection(direction) || !IsGoalDirection(goalDirection) ||
|
||
!NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) ||
|
||
!NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) ||
|
||
!NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
|
||
!NumericGuard.IsPositiveFinite(map.ResolutionMeters) ||
|
||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||
return false;
|
||
|
||
return Math.Abs(curvaturePerMeter) <= maximumCurvaturePerMeter;
|
||
}
|
||
|
||
private static Pose2D Integrate(Pose2D previous, double curvaturePerMeter, TravelDirection direction, double stepMeters)
|
||
{
|
||
double signedDistanceMeters = direction == TravelDirection.Forward ? stepMeters : -stepMeters;
|
||
double nextHeadingRadians = AngleMath.NormalizeRadians(previous.Heading + curvaturePerMeter * signedDistanceMeters);
|
||
if (Math.Abs(curvaturePerMeter) < StraightCurvatureThreshold)
|
||
{
|
||
return new Pose2D(
|
||
previous.X + signedDistanceMeters * Math.Cos(previous.Heading),
|
||
previous.Y + signedDistanceMeters * Math.Sin(previous.Heading),
|
||
nextHeadingRadians);
|
||
}
|
||
|
||
return new Pose2D(
|
||
previous.X + (Math.Sin(nextHeadingRadians) - Math.Sin(previous.Heading)) / curvaturePerMeter,
|
||
previous.Y - (Math.Cos(nextHeadingRadians) - Math.Cos(previous.Heading)) / curvaturePerMeter,
|
||
nextHeadingRadians);
|
||
}
|
||
|
||
private static bool IsFinitePose(Pose2D pose)
|
||
{
|
||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||
}
|
||
|
||
private static bool IsTravelDirection(TravelDirection direction)
|
||
{
|
||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||
}
|
||
|
||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||
{
|
||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||
}
|
||
}
|