100 lines
4.0 KiB
C#
100 lines
4.0 KiB
C#
using System;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||
|
||
/// <summary>
|
||
/// Hybrid A* 运行期节点。
|
||
/// 节点保留连续位姿和其离散闭集键；搜索过程中不会修改已创建节点，改进代价时会追加新节点并使旧 Open List 条目失效。
|
||
/// </summary>
|
||
public sealed class HybridAStarNode
|
||
{
|
||
/// <summary>
|
||
/// 创建一个搜索节点。
|
||
/// 参数：nodeIndex 为本次搜索内稳定索引；key 为离散状态；pose 为连续车辆中心位姿；parentNodeIndex 为父节点索引，根节点使用 -1；
|
||
/// incomingPrimitive 为父节点到本节点的原语，根节点为 null；curvaturePerMeter、gCostMeters 和 hCostMeters 均使用规划器的标准单位。
|
||
/// </summary>
|
||
public HybridAStarNode(
|
||
int nodeIndex,
|
||
HybridAStarNodeKey key,
|
||
Pose2D pose,
|
||
int parentNodeIndex,
|
||
MotionPrimitive incomingPrimitive,
|
||
double curvaturePerMeter,
|
||
double gCostMeters,
|
||
double hCostMeters)
|
||
: this(nodeIndex, key, pose, parentNodeIndex, incomingPrimitive, curvaturePerMeter, gCostMeters, hCostMeters, false)
|
||
{
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建一个搜索节点，并显式指定它是否为原语内部命中的终点候选。
|
||
/// 参数：isGoalCandidate 为 true 时，本节点不参与普通离散状态的最优 G 值支配；它仍必须在从 Open List 出队后重新通过连续终点和碰撞检查才能成功。
|
||
/// </summary>
|
||
public HybridAStarNode(
|
||
int nodeIndex,
|
||
HybridAStarNodeKey key,
|
||
Pose2D pose,
|
||
int parentNodeIndex,
|
||
MotionPrimitive incomingPrimitive,
|
||
double curvaturePerMeter,
|
||
double gCostMeters,
|
||
double hCostMeters,
|
||
bool isGoalCandidate)
|
||
{
|
||
if (key == null) throw new ArgumentNullException(nameof(key));
|
||
if (pose == null) throw new ArgumentNullException(nameof(pose));
|
||
|
||
NodeIndex = nodeIndex;
|
||
Key = key;
|
||
Pose = pose;
|
||
ParentNodeIndex = parentNodeIndex;
|
||
IncomingPrimitive = incomingPrimitive;
|
||
CurvaturePerMeter = curvaturePerMeter;
|
||
GCostMeters = gCostMeters;
|
||
HCostMeters = hCostMeters;
|
||
IsGoalCandidate = isGoalCandidate;
|
||
}
|
||
|
||
/// <summary>本次搜索节点数组中的稳定索引。</summary>
|
||
public int NodeIndex { get; }
|
||
|
||
/// <summary>本节点用于 Open/Closed 状态管理的离散键。</summary>
|
||
public HybridAStarNodeKey Key { get; }
|
||
|
||
/// <summary>未量化的连续车辆中心位姿。</summary>
|
||
public Pose2D Pose { get; }
|
||
|
||
/// <summary>父节点稳定索引；根节点为 -1。</summary>
|
||
public int ParentNodeIndex { get; }
|
||
|
||
/// <summary>由父节点驶入本节点的已连续碰撞检查原语；根节点为 null。</summary>
|
||
public MotionPrimitive IncomingPrimitive { get; }
|
||
|
||
/// <summary>与 <see cref="IncomingPrimitive"/> 含义相同的原语别名。</summary>
|
||
public MotionPrimitive Primitive { get { return IncomingPrimitive; } }
|
||
|
||
/// <summary>当前末段采用的车辆曲率，单位 1/m。</summary>
|
||
public double CurvaturePerMeter { get; }
|
||
|
||
/// <summary>当前节点的累计等效米代价。</summary>
|
||
public double GCostMeters { get; }
|
||
|
||
/// <summary>当前节点的二维启发式等效米代价。</summary>
|
||
public double HCostMeters { get; }
|
||
|
||
/// <summary>
|
||
/// 本节点是否由原语内部首次满足终点条件而生成。
|
||
/// 终点候选必须保留连续位姿，不能因同一离散键的普通低代价节点而被 Open List 准入规则压制。
|
||
/// </summary>
|
||
public bool IsGoalCandidate { get; }
|
||
|
||
/// <summary>Open List 排序使用的总等效米代价。</summary>
|
||
public double FCostMeters { get { return GCostMeters + HCostMeters; } }
|
||
|
||
/// <summary>本节点末段的行驶方向。</summary>
|
||
public TravelDirection Direction { get { return Key.Direction; } }
|
||
|
||
/// <summary>本节点末段的曲率等级索引。</summary>
|
||
public int CurvatureLevelIndex { get { return Key.CurvatureLevelIndex; } }
|
||
}
|