zengzy / hhrcs_service · Files

using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Extensions.Logging;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind.Models;
using Rcs.Domain.ValueObjects;
using Rcs.Shared.Utils;

namespace Rcs.Infrastructure.PathFinding.Services
{
    /// <summary>
    /// A*寻路算法实现(考虑全局路径冲突、倒车约束)
    /// @author zzy
    /// </summary>
    public class AStarPathFinder
    {
        private readonly ILogger<AStarPathFinder> _logger;
        private readonly AStarPathFinder _pathFinder;

        /// <summary>
        /// 同向冲突代价
        /// </summary>
        private const double SameDirectionCost = 5.0;

        /// <summary>
        /// 对向冲突代价(死锁风险)
        /// </summary>
        private const double OppositeDirectionCost = 100.0;

        public AStarPathFinder(ILogger<AStarPathFinder> logger)
        {
            _logger = logger;
            
        }

        /// <summary>
        /// 计算最优路径(考虑全局已规划路径)
        /// @author zzy
        /// 2026-02-07 更新：支持起点等于终点时返回原地路径
        /// </summary>
        public PathResult FindPath(PathRequest request, PathGraph graph, GlobalPathContext? globalContext = null)
        {
            var sw = System.Diagnostics.Stopwatch.StartNew();
            var result = new PathResult();

            if (!graph.Nodes.TryGetValue(request.StartNodeId, out var startNode))
            {
                result.ErrorMessage = "起始节点不存在";
                return result;
            }

            if (!graph.Nodes.TryGetValue(request.EndNodeId, out var endNode))
            {
                result.ErrorMessage = "目标节点不存在";
                return result;
            }

            // 计算终点要求的车头朝向(弧度)
            // 货叉取货方向需要和终点朝向相反，车头方向 = 终点朝向 + π - 货叉夹角
            List<double> requiredEndAngles = new List<double>();
            if (request.ForkRadOffsets != null && request.ForkRadOffsets.Count > 0)
            {
                foreach(var angle in request.ForkRadOffsets)
                {
                    requiredEndAngles.Add(request.RequiredEndRad.Value + Math.PI - angle);
                }
            }

            // 特殊情况：起点等于终点
            // 检查当前朝向是否满足终点约束，如果满足则返回原地路径
            if (request.StartNodeId == request.EndNodeId)
            {
                var currentTheta = AngleConverter.NormalizeRadians(request.CurrentTheta);
                bool angleMatched = true;

                // 如果有终点朝向约束，检查当前朝向是否满足
                if (requiredEndAngles.Count > 0)
                {
                    angleMatched = false;
                    foreach (var requiredAngle in requiredEndAngles)
                    {
                        var diff = Math.Abs(AngleConverter.NormalizeRadians(currentTheta - requiredAngle));
                        if (diff <= AngleConstants.Degrees15)
                        {
                            angleMatched = true;
                            break;
                        }
                    }
                }
                // 情况1：朝向已满足约束，直接返回原地路径
                // 情况2：朝向不满足但终点允许旋转，返回原地路径（车辆可原地旋转调整朝向）
                if (angleMatched || startNode.AllowRotate)
                {
                    result.Success = true;
                    // 返回包含一个原地路径段的列表（起点到起点，长度为0）
                    result.Segments = new List<PathSegment>
                    {
                        new PathSegment
                        {
                            EdgeId = Guid.Empty,  // 无实际边
                            FromNodeId = request.StartNodeId,
                            ToNodeId = request.StartNodeId,
                            Angle = currentTheta,
                            Length = 0,
                            MaxSpeed = null
                        }
                    };
                    result.TotalLength = 0;
                    result.TotalCost = 0;
                    sw.Stop();
                    result.ComputeTimeMs = sw.ElapsedMilliseconds;

                    if (angleMatched)
                    {
                        _logger.LogInformation("起点等于终点且朝向满足约束，返回原地路径: StartNodeId={StartNodeId}", request.StartNodeId);
                    }
                    else
                    {
                        _logger.LogInformation("起点等于终点，朝向不满足但节点允许旋转，返回原地路径: StartNodeId={StartNodeId}, AllowRotate={AllowRotate}",
                            request.StartNodeId, startNode.AllowRotate);
                    }
                    return result;
                }
                // 朝向不满足且不允许旋转，继续正常寻路（可能需要绕一圈调整朝向）
                //_logger.LogInformation("起点等于终点但朝向不满足约束且不允许旋转，尝试寻找调整朝向的路径: StartNodeId={StartNodeId}", request.StartNodeId);
            }

            var openSet = new PriorityQueue<SearchNode, double>();
            var closedSet = new HashSet<Guid>();
            var gScore = new Dictionary<Guid, double> { [request.StartNodeId] = 0 };
            var cameFrom = new Dictionary<Guid, (Guid nodeId, PathEdge edge, double Angle)>();

            openSet.Enqueue(new SearchNode
            {
                NodeId = request.StartNodeId,
                currentTheta = AngleConverter.NormalizeRadians(request.CurrentTheta),
                GScore = 0,
                FScore = Heuristic(startNode, endNode)
            }, Heuristic(startNode, endNode));

            while (openSet.Count > 0)
            {
                var current = openSet.Dequeue();

                // 终点约束
                if (current.NodeId == request.EndNodeId)
                {
                    // 检查终点朝向约束（允许 AngleConstants.Degrees15 的偏移）
                    // 如果终点允许旋转，则跳过朝向检查（车辆可原地旋转调整）
                    bool canAcceptEndpoint = true;

                    if (requiredEndAngles.Count > 0 && !endNode.AllowRotate)
                    {
                        bool angleMatched = false;

                        foreach (var requiredAngle in requiredEndAngles)
                        {
                            // 计算角度差，使用 AngleConstants.Degrees15（弧度）作为容差
                            var diff = Math.Abs(AngleConverter.NormalizeRadians(
                                current.currentTheta - requiredAngle));
                            if (diff <= AngleConstants.Degrees15)
                            {
                                angleMatched = true;
                                break;
                            }
                        }

                        if (!angleMatched)
                        {
                            // 朝向不满足且终点不允许旋转，继续搜索其他路径
                            canAcceptEndpoint = false;
                        }
                    }

                    if (canAcceptEndpoint)
                    {
                        result.Success = true;
                        result.Segments = ReconstructPath(cameFrom, request.EndNodeId);
                        result.TotalLength = result.Segments.Sum(s => s.Length);
                        result.TotalCost = gScore[request.EndNodeId];
                        break;
                    }
                    else
                    {
                        // 朝向不满足，继续搜索其他路径
                        continue;
                    }
                }

                if (closedSet.Contains(current.NodeId)) continue;
                closedSet.Add(current.NodeId);

                var currentNode = graph.Nodes[current.NodeId];

                foreach (var edge in currentNode.OutEdges)
                {
                    if (!edge.Active) continue;
                    var neighborId = edge.ToNodeId;

                    if (!graph.Nodes.TryGetValue(neighborId, out var neighborNode)) continue;
                    if (!neighborNode.Active) continue;

                    // 计算边支持方向和车辆能力集方向的交集，并找到当前航向角±偏差范围内最接近的行驶方向
                    var (selectedRad, needsRotation) = SelectBestTraversalRad(
                        edge.OrientationRads, request.VehicleOrientationAngles,
                        current.currentTheta, edge.StartTangentRad, edge.MaxRadDeviation);

                    // 无交集则跳过
                    if (!selectedRad.HasValue) continue;

                    // 有交集但需要旋转，检查起点是否支持旋转
                    if (needsRotation && !currentNode.AllowRotate) continue;

                    // 计算全局路径冲突代价（传入当前累计代价用于时间窗口分析）
                    // 这里用累计代价近似估算时间，假设代价与距离正相关
                    var conflictCost = CalculateConflictCost(edge, globalContext, request.RobotId, gScore[current.NodeId]);

                    // 计算终点车辆全局航向角(弧度)
                    var newAngle = selectedRad.Value + edge.DirectionRad;

                    var tentativeG = gScore[current.NodeId] + edge.Cost + conflictCost;

                    if (!gScore.TryGetValue(neighborId, out var existingG) || tentativeG < existingG)
                    {
                        gScore[neighborId] = tentativeG;
                        cameFrom[neighborId] = (current.NodeId, edge, selectedRad.Value);

                        var fScore = tentativeG + Heuristic(neighborNode, endNode);

                        openSet.Enqueue(new SearchNode
                        {
                            NodeId = neighborId,
                            currentTheta = AngleConverter.NormalizeRadians(newAngle) ,
                            GScore = tentativeG,
                            FScore = fScore
                        }, fScore);
                    }
                }
            }

            if (!result.Success)
            {
                result.ErrorMessage = "无法找到可行路径";
            }

            sw.Stop();
            result.ComputeTimeMs = sw.ElapsedMilliseconds;
            return result;
        }

        /// <summary>
        /// 计算全局路径冲突代价（基于时间窗口分析，支持跨地图冲突检测）
        /// @author zzy
        /// </summary>
        /// <param name="edge">当前要评估的边</param>
        /// <param name="context">全局路径上下文</param>
        /// <param name="currentRobotId">当前机器人ID</param>
        /// <param name="accumulatedCost">从起点到当前边起点的累计代价（注意：是代价不是距离）</param>
        /// <returns>冲突代价</returns>
        /// <remarks>
        /// 注意：此方法使用累计代价近似估算到达时间。
        /// 如果 edge.Cost ≈ edge.Length，则估算较为准确；
        /// 如果代价包含大量非距离因素，估算会有偏差，但仍可用于相对排序。
        /// </remarks>
        private double CalculateConflictCost(PathEdge edge, GlobalPathContext? context, Guid currentRobotId, double accumulatedCost)
        {
            if (context == null) return 0;

            // 获取当前机器人的平均速度，默认1.0米/秒
            double myAverageSpeed = context.RobotAverageSpeeds.TryGetValue(currentRobotId, out var speed)
                ? speed
                : 1.0;

            // 如果速度为0或负数，使用默认值
            if (myAverageSpeed <= 0) myAverageSpeed = 1.0;

            // 估算到达该边入口的时间（使用累计代价近似距离）
            // T_arrival ≈ AccumulatedCost / V_me
            // 注意：如果代价与距离不成正比，此估算会有偏差
            double tArrival = accumulatedCost / myAverageSpeed;

            double cost = 0;
            foreach (var (robotId, plannedEdges) in context.RobotPlannedEdges)
            {
                if (robotId == currentRobotId) continue;

                foreach (var planned in plannedEdges)
                {
                    // 检查冲突：优先使用 NodeCode 匹配（跨地图），否则使用 NodeId 匹配（同地图）
                    var conflictType = DetectEdgeConflict(edge, planned);

                    if (conflictType == EdgeConflictType.SameDirection)
                    {
                        // 同向冲突：两车走同一条边
                        // 计算前车离开该边的时间
                        double tLeave = CalculateTimeToLeaveEdge(planned);

                        // 安全跟车时间（秒）- 同向跟车需要更短的安全距离
                        const double followSafetyBuffer = 3.0;

                        // 等我到了，前车早走了
                        if (tArrival > tLeave + followSafetyBuffer)
                        {
                            // 无冲突，代价为1
                            cost += 1;
                        }
                        // 我会追上前车或紧跟其后
                        else if (tArrival <= tLeave)
                        {
                            // 需要跟车，给予较小惩罚代价
                            cost += SameDirectionCost;
                        }
                        // 时间窗口接近，可能需要减速跟车
                        else
                        {
                            // 代价与时间差成反比：时间差越小，代价越高
                            double timeDiff = tArrival - tLeave;
                            double followCost = SameDirectionCost * (1 - timeDiff / followSafetyBuffer);
                            cost += followCost;
                        }
                    }
                    else if (conflictType == EdgeConflictType.OppositeDirection)
                    {
                        // 对向冲突：逆向车在该边上
                        // 计算逆向车离开该边的时间
                        double tLeave = CalculateTimeToLeaveEdge(planned);

                        // 安全缓冲时间（秒）
                        const double safetyBuffer = 5.0;

                        if (tArrival > tLeave + safetyBuffer)
                        {
                            cost += SameDirectionCost;
                        }
                        else if (tArrival <= tLeave)
                        {
                            // 真正的死锁风险，较大代价
                            cost += OppositeDirectionCost;
                        }
                        // 时间窗口重叠但有安全缓冲（T_leave < T_arrival <= T_leave + 安全缓冲）
                        else
                        {
                            // 可能存在冲突，给予较高代价但不是无穷大
                            // 代价与时间差成反比：时间差越小，代价越高
                            double timeDiff = tArrival - tLeave;
                            double conflictCost = OppositeDirectionCost * (1 - timeDiff / safetyBuffer);
                            cost += conflictCost;
                        }
                    }
                }
            }
            return cost;
        }

        /// <summary>
        /// 检测两条边的冲突类型（支持跨地图检测）
        /// @author zzy
        /// </summary>
        /// <param name="currentEdge">当前要评估的边</param>
        /// <param name="plannedEdge">其他机器人已规划的边</param>
        /// <returns>冲突类型</returns>
        private static EdgeConflictType DetectEdgeConflict(PathEdge currentEdge, PlannedEdge plannedEdge)
        {
            // 优先使用 NodeCode 进行匹配（跨地图场景）
            bool hasNodeCodes = !string.IsNullOrEmpty(currentEdge.FromNodeCode) &&
                                !string.IsNullOrEmpty(currentEdge.ToNodeCode) &&
                                !string.IsNullOrEmpty(plannedEdge.FromNodeCode) &&
                                !string.IsNullOrEmpty(plannedEdge.ToNodeCode);

            if (hasNodeCodes)
            {
                // 同向冲突（基于 NodeCode）
                if (string.Equals(plannedEdge.FromNodeCode, currentEdge.FromNodeCode, StringComparison.OrdinalIgnoreCase) &&
                    string.Equals(plannedEdge.ToNodeCode, currentEdge.ToNodeCode, StringComparison.OrdinalIgnoreCase))
                {
                    return EdgeConflictType.SameDirection;
                }

                // 对向冲突（基于 NodeCode）
                if (string.Equals(plannedEdge.FromNodeCode, currentEdge.ToNodeCode, StringComparison.OrdinalIgnoreCase) &&
                    string.Equals(plannedEdge.ToNodeCode, currentEdge.FromNodeCode, StringComparison.OrdinalIgnoreCase))
                {
                    return EdgeConflictType.OppositeDirection;
                }
            }
            else
            {
                // 回退到 NodeId 匹配（同地图场景）
                // 同向冲突
                if (plannedEdge.FromNodeId == currentEdge.FromNodeId && plannedEdge.ToNodeId == currentEdge.ToNodeId)
                {
                    return EdgeConflictType.SameDirection;
                }

                // 对向冲突
                if (plannedEdge.FromNodeId == currentEdge.ToNodeId && plannedEdge.ToNodeId == currentEdge.FromNodeId)
                {
                    return EdgeConflictType.OppositeDirection;
                }
            }

            return EdgeConflictType.None;
        }

        /// <summary>
        /// 计算机器人离开边的时间（秒）
        /// @author zzy
        /// </summary>
        /// <param name="planned">已规划的边及机器人状态</param>
        /// <returns>离开时间（秒），如果被阻塞则返回无穷大</returns>
        private static double CalculateTimeToLeaveEdge(PlannedEdge planned)
        {
            // 车辆被阻塞（遇到人或障碍物）
            if (planned.IsBlocked)
            {
                // 被堵住了，离开时间趋近无穷大
                return double.MaxValue;
            }

            // 车辆还未到达该边（TraveledDistance == 0）
            if (planned.TraveledDistance <= 0)
            {
                // 车辆还在路上，需要先到达该边，再穿过该边
                // T_leave = T_arrival + T_traverse
                // T_arrival = EstimatedArrivalTime（到达边起点的时间）
                // T_traverse = Length / AverageSpeed（穿过边的时间）
                double effectiveSpeed = planned.AverageSpeed > 0 ? planned.AverageSpeed : 1.0;
                double traverseTime = planned.Length / effectiveSpeed;
                return planned.EstimatedArrivalTime + traverseTime;
            }

            // 车辆已在边上（TraveledDistance > 0）
            // 计算剩余距离
            double remainingDistance = planned.Length - planned.TraveledDistance;

            // 如果已经走完或超出边长，认为即将离开
            if (remainingDistance <= 0)
            {
                return 0;
            }

            // 优先使用当前速度，如果当前速度为0或负数，使用平均速度
            double effectiveSpeed2 = planned.CurrentSpeed > 0
                ? planned.CurrentSpeed
                : planned.AverageSpeed;

            // 如果速度仍然为0或负数，认为车辆停止，离开时间无穷大
            if (effectiveSpeed2 <= 0)
            {
                return double.MaxValue;
            }

            // T_leave = D_remain / V_other
            double tLeave = remainingDistance / effectiveSpeed2;

            return tLeave;
        }

        /// <summary>
        /// 选择最佳行驶角度：计算边和车辆方向交集，找到当前航向角±偏差范围内最接近的值
        /// @author zzy
        /// </summary>
        /// <param name="edgeRads">边支持的行驶方向(弧度，相对于边切线)</param>
        /// <param name="vehicleRads">车辆支持的行驶方向(弧度，相对于车头)</param>
        /// <param name="currentTheta">当前全局航向角(弧度)</param>
        /// <param name="edgeDirection">边切线方向(弧度，全局坐标系)</param>
        /// <param name="maxDeviation">最大允许偏差(弧度)</param>
        /// <returns>(最佳行驶角度, 是否需要旋转)，无交集时角度为null</returns>
        private static (double? selectedRad, bool needsRotation) SelectBestTraversalRad(
            List<double> edgeRads, List<double> vehicleRads,
            double currentTheta, double edgeDirection, double? maxDeviation)
        {
            // 默认支持正向(0)和倒车(π)
            var edges = edgeRads.Count > 0 ? edgeRads : new List<double> { 0, Math.PI };
            var vehicles = vehicleRads.Count > 0 ? vehicleRads : new List<double> { 0, Math.PI };
            var maxDev = maxDeviation ?? Math.PI; // 默认为180度（π弧度）

            // 计算交集
            var intersection = edges.Where(ea => vehicles.Any(va => Math.Abs(AngleConverter.NormalizeRadians(va - ea)) < 0.001)).ToList();
            if (intersection.Count == 0) return (null, false);

            // 找偏差范围内最接近当前航向角的
            double? bestAngle = null;
            double minDiff = double.MaxValue;

            foreach (var angle in intersection)
            {
                // 将相对行驶角度转换为全局航向角
                var globalHeading = AngleConverter.NormalizeRadians(edgeDirection + angle);
                var deviation = Math.Abs(AngleConverter.NormalizeRadians(currentTheta - globalHeading));
                if (deviation <= maxDev && deviation < minDiff)
                {
                    minDiff = deviation;
                    bestAngle = angle;
                }
            }

            // 偏差范围内无命中，需要旋转，选最接近的
            if (!bestAngle.HasValue)
            {
                foreach (var angle in intersection)
                {
                    var globalHeading = AngleConverter.NormalizeRadians(edgeDirection + angle);
                    var deviation = Math.Abs(AngleConverter.NormalizeRadians(currentTheta - globalHeading));
                    if (deviation < minDiff)
                    {
                        minDiff = deviation;
                        bestAngle = angle;
                    }
                }
                return (bestAngle, true);
            }

            return (bestAngle, false);
        }

        /// <summary>
        /// 计算转向代价
        /// @author zzy
        /// </summary>
        /// <param name="currentTheta">当前航向角(弧度)</param>
        /// <param name="targetAngle">目标航向角(弧度)</param>
        /// <param name="allowRotate">是否允许旋转</param>
        /// <returns>转向代价(0-1)</returns>
        private double CalculateTurnCost(double currentTheta, double targetAngle, bool allowRotate)
        {
            var diff = Math.Abs(NormalizeAngle(currentTheta - targetAngle));
            if (!allowRotate && diff > 0.1) return double.MaxValue;
            return diff / Math.PI;
        }

        /// <summary>
        /// 启发式函数(欧几里得距离)
        /// @author zzy
        /// </summary>
        /// <param name="from">起点节点</param>
        /// <param name="to">终点节点</param>
        /// <returns>两点间欧几里得距离</returns>
        private double Heuristic(PathNode from, PathNode to)
        {
            var dx = to.X - from.X;
            var dy = to.Y - from.Y;
            return Math.Sqrt(dx * dx + dy * dy);
        }

        /// <summary>
        /// 回溯构建路径
        /// @author zzy
        /// </summary>
        /// <param name="cameFrom">父节点记录</param>
        /// <param name="endNodeId">终点节点ID</param>
        /// <returns>路径段列表</returns>
        private List<PathSegment> ReconstructPath(
            Dictionary<Guid, (Guid nodeId, PathEdge edge, double Angle)> cameFrom, Guid endNodeId)
        {
            var segments = new List<PathSegment>();
            var current = endNodeId;

            while (cameFrom.TryGetValue(current, out var prev))
            {
                segments.Add(new PathSegment
                {
                    EdgeId = prev.edge.EdgeId,
                    FromNodeId = prev.nodeId,
                    ToNodeId = current,
                    Angle = prev.Angle,
                    Length = prev.edge.Length,
                    MaxSpeed = prev.edge.MaxSpeed
                });
                current = prev.nodeId;
            }

            segments.Reverse();
            return segments;
        }

        /// <summary>
        /// 角度归一化到[-PI, PI]
        /// @author zzy
        /// </summary>
        /// <param name="angle">输入角度(弧度)</param>
        /// <returns>归一化后的角度(弧度, -PI到PI)</returns>
        private double NormalizeAngle(double angle)
        {
            while (angle > Math.PI) angle -= 2 * Math.PI;
            while (angle < -Math.PI) angle += 2 * Math.PI;
            return angle;
        }

        /// <summary>
        /// 角度归一化到[-180, 180](度)
        /// @author zzy
        /// </summary>
        private static double NormalizeDegreesSymmetric(double degrees)
        {
            while (degrees > 180) degrees -= 360;
            while (degrees < -180) degrees += 360;
            return degrees;
        }

        private class SearchNode
        {
            public Guid NodeId { get; set; }
            public double currentTheta { get; set; }
            public double GScore { get; set; }
            public double FScore { get; set; }
        }
    }

    /// <summary>
    /// 边冲突类型
    /// @author zzy
    /// </summary>
    public enum EdgeConflictType
    {
        /// <summary>无冲突</summary>
        None = 0,
        /// <summary>同向冲突（跟车）</summary>
        SameDirection = 1,
        /// <summary>对向冲突（死锁风险）</summary>
        OppositeDirection = 2
    }



    

    
}