62 lines
3.0 KiB
C#
62 lines
3.0 KiB
C#
using System;
|
||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||
|
||
/// <summary>按位置、航向和进入方向约束判定连续位姿是否可以作为目标候选。</summary>
|
||
public static class GoalToleranceChecker
|
||
{
|
||
/// <summary>
|
||
/// 判断一个位姿是否满足目标容差和进入方向约束。
|
||
/// 参数：pose 与 goal 使用世界 m/rad；configuration 提供位置 m 和航向 rad 容差；direction 为候选末段方向；goalDirection 为目标进入方向约束。
|
||
/// 返回：输入有限、位置距离和最小环形航向误差均不超过容差且方向匹配时为 true；无效输入保守地返回 false。
|
||
/// </summary>
|
||
public static bool IsSatisfied(
|
||
Pose2D pose,
|
||
Pose2D goal,
|
||
HybridAStarConfiguration configuration,
|
||
TravelDirection direction,
|
||
GoalDirectionConstraint goalDirection)
|
||
{
|
||
if (!IsFinitePose(pose) || !IsFinitePose(goal) || configuration == null ||
|
||
!NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) ||
|
||
!NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) ||
|
||
configuration.GoalPositionToleranceMeters < 0d || configuration.GoalHeadingToleranceRadians < 0d ||
|
||
!IsTravelDirection(direction) || !IsGoalDirection(goalDirection))
|
||
return false;
|
||
|
||
if (!MatchesDirection(direction, goalDirection)) return false;
|
||
|
||
double deltaX = pose.X - goal.X;
|
||
double deltaY = pose.Y - goal.Y;
|
||
double positionDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||
if (!NumericGuard.IsFinite(positionDistanceMeters) || positionDistanceMeters > configuration.GoalPositionToleranceMeters)
|
||
return false;
|
||
|
||
double headingDifferenceRadians = Math.Abs(AngleMath.ShortestSignedDifference(pose.Heading, goal.Heading));
|
||
return NumericGuard.IsFinite(headingDifferenceRadians) && headingDifferenceRadians <= configuration.GoalHeadingToleranceRadians;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
private static bool MatchesDirection(TravelDirection direction, GoalDirectionConstraint constraint)
|
||
{
|
||
return constraint == GoalDirectionConstraint.Any ||
|
||
(constraint == GoalDirectionConstraint.Forward && direction == TravelDirection.Forward) ||
|
||
(constraint == GoalDirectionConstraint.Reverse && direction == TravelDirection.Reverse);
|
||
}
|
||
}
|