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);
|
|||
|
|
}
|
|||
|
|
}
|