Add NPC stuck detection (#14410)

Content.Server/NPC/Components/NPCSteeringComponent.cs

@@ -2,6 +2,7 @@ using System.Threading;
 using Content.Server.NPC.Pathfinding;
 using Content.Shared.NPC;
 using Robust.Shared.Map;
+using Robust.Shared.Serialization.TypeSerializers.Implementations.Custom;
 
 namespace Content.Server.NPC.Components;
 
@@ -39,12 +40,25 @@ public sealed class NPCSteeringComponent : Component
     /// <summary>
     /// Next time we can change our steering direction.
     /// </summary>
+    [DataField("nextSteer", customTypeSerializer:typeof(TimeOffsetSerializer))]
     public TimeSpan NextSteer = TimeSpan.Zero;
 
+    [DataField("lastSteerDirection")]
     public Vector2 LastSteerDirection = Vector2.Zero;
 
     public const int SteeringFrequency = 10;
 
+    /// <summary>
+    /// Last position we considered for being stuck.
+    /// </summary>
+    [DataField("lastStuckCoordinates")]
+    public EntityCoordinates LastStuckCoordinates;
+
+    [DataField("lastStuckTime", customTypeSerializer:typeof(TimeOffsetSerializer))]
+    public TimeSpan LastStuckTime;
+
+    public const float StuckDistance = 0.5f;
+
     /// <summary>
     /// Have we currently requested a path.
     /// </summary>

Content.Server/NPC/Systems/NPCSteeringSystem.Context.cs

@@ -45,7 +45,8 @@ public sealed partial class NPCSteeringSystem
         float moveSpeed,
         float[] interest,
         EntityQuery<PhysicsComponent> bodyQuery,
-        float frameTime)
+        float frameTime,
+        ref bool forceSteer)
     {
         var ourCoordinates = xform.Coordinates;
         var destinationCoordinates = steering.Coordinates;
@@ -72,6 +73,7 @@ public sealed partial class NPCSteeringSystem
                 // Try to get the next node temporarily.
                 targetCoordinates = GetTargetCoordinates(steering);
                 needsPath = true;
+                ResetStuck(steering, ourCoordinates);
             }
         }
 
@@ -84,14 +86,22 @@ public sealed partial class NPCSteeringSystem
             // If it's a pathfinding node it might be different to the destination.
             arrivalDistance = steering.Range;
         }
+        // If next node is a free tile then get within its bounds.
+        // This is to avoid popping it too early
+        else if (steering.CurrentPath.TryPeek(out var node) && node.Data.IsFreeSpace)
+        {
+            arrivalDistance = MathF.Min(node.Box.Width, node.Box.Height) - 0.01f;
+        }
+        // Try getting into blocked range I guess?
+        // TODO: Consider melee range or the likes.
         else
         {
             arrivalDistance = SharedInteractionSystem.InteractionRange - 0.65f;
         }
 
         // Check if mapids match.
-        var targetMap = targetCoordinates.ToMap(EntityManager);
+        var targetMap = targetCoordinates.ToMap(EntityManager, _transform);
-        var ourMap = ourCoordinates.ToMap(EntityManager);
+        var ourMap = ourCoordinates.ToMap(EntityManager, _transform);
 
         if (targetMap.MapId != ourMap.MapId)
         {
@@ -107,6 +117,8 @@ public sealed partial class NPCSteeringSystem
             // Node needs some kind of special handling like access or smashing.
             if (steering.CurrentPath.TryPeek(out var node) && !node.Data.IsFreeSpace)
             {
+                // Ignore stuck while handling obstacles.
+                ResetStuck(steering, ourCoordinates);
                 SteeringObstacleStatus status;
 
                 // Breaking behaviours and the likes.
@@ -125,25 +137,24 @@ public sealed partial class NPCSteeringSystem
                         steering.Status = SteeringStatus.NoPath;
                         return false;
                     case SteeringObstacleStatus.Continuing:
-                        CheckPath(steering, xform, needsPath, distance);
+                        CheckPath(uid, steering, xform, needsPath, distance);
                         return true;
                     default:
                         throw new ArgumentOutOfRangeException();
                 }
             }
 
-            // Otherwise it's probably regular pathing so just keep going a bit more to get to tile centre
+            // Distance should already be handled above.
-            if (direction.Length <= TileTolerance)
-            {
             // It was just a node, not the target, so grab the next destination (either the target or next node).
             if (steering.CurrentPath.Count > 0)
             {
+                forceSteer = true;
                 steering.CurrentPath.Dequeue();
 
                 // Alright just adjust slightly and grab the next node so we don't stop moving for a tick.
                 // TODO: If it's the last node just grab the target instead.
                 targetCoordinates = GetTargetCoordinates(steering);
-                    targetMap = targetCoordinates.ToMap(EntityManager);
+                targetMap = targetCoordinates.ToMap(EntityManager, _transform);
 
                 // Can't make it again.
                 if (ourMap.MapId != targetMap.MapId)
@@ -155,6 +166,7 @@ public sealed partial class NPCSteeringSystem
 
                 // Gonna resume now business as usual
                 direction = targetMap.Position - ourMap.Position;
+                ResetStuck(steering, ourCoordinates);
             }
             else
             {
@@ -163,6 +175,30 @@ public sealed partial class NPCSteeringSystem
                 return false;
             }
         }
+        // Stuck detection
+        // Check if we have moved further than the movespeed * stuck time.
+        else if (ourCoordinates.TryDistance(EntityManager, steering.LastStuckCoordinates, out var stuckDistance) &&
+                 stuckDistance < NPCSteeringComponent.StuckDistance)
+        {
+            var stuckTime = _timing.CurTime - steering.LastStuckTime;
+            // Either 1 second or how long it takes to move the stuck distance + buffer if we're REALLY slow.
+            var maxStuckTime = Math.Max(1, NPCSteeringComponent.StuckDistance / moveSpeed * 1.2f);
+
+            if (stuckTime.TotalSeconds > maxStuckTime)
+            {
+                // TODO: Blacklist nodes (pathfinder factor wehn)
+                // TODO: This should be a warning but
+                // A) NPCs get stuck on non-anchored static bodies still (e.g. closets)
+                // B) NPCs still try to move in locked containers (e.g. cow, hamster)
+                // and I don't want to spam grafana even harder than it gets spammed rn.
+                _sawmill.Debug($"NPC {ToPrettyString(uid)} found stuck at {ourCoordinates}");
+                steering.Status = SteeringStatus.NoPath;
+                return false;
+            }
+        }
+        else
+        {
+            ResetStuck(steering, ourCoordinates);
         }
 
         // Do we have no more nodes to follow OR has the target moved sufficiently? If so then re-path.
@@ -172,7 +208,7 @@ public sealed partial class NPCSteeringSystem
         }
 
         // TODO: Probably need partial planning support i.e. patch from the last node to where the target moved to.
-        CheckPath(steering, xform, needsPath, distance);
+        CheckPath(uid, steering, xform, needsPath, distance);
 
         // If we don't have a path yet then do nothing; this is to avoid stutter-stepping if it turns out there's no path
         // available but we assume there was.
@@ -198,17 +234,22 @@ public sealed partial class NPCSteeringSystem
         // Prefer our current direction
         if (weight > 0f && body.LinearVelocity.LengthSquared > 0f)
         {
-            const float SameDirectionWeight = 0.1f;
+            const float sameDirectionWeight = 0.1f;
             norm = body.LinearVelocity.Normalized;
 
-            ApplySeek(interest, norm, SameDirectionWeight);
+            ApplySeek(interest, norm, sameDirectionWeight);
         }
 
         return true;
     }
 
+    private void ResetStuck(NPCSteeringComponent component, EntityCoordinates ourCoordinates)
+    {
+        component.LastStuckCoordinates = ourCoordinates;
+        component.LastStuckTime = _timing.CurTime;
+    }
 
-    private void CheckPath(NPCSteeringComponent steering, TransformComponent xform, bool needsPath, float targetDistance)
+    private void CheckPath(EntityUid uid, NPCSteeringComponent steering, TransformComponent xform, bool needsPath, float targetDistance)
     {
         if (!_pathfinding)
         {
@@ -233,7 +274,7 @@ public sealed partial class NPCSteeringSystem
         // Request the new path.
         if (needsPath)
         {
-            RequestPath(steering, xform, targetDistance);
+            RequestPath(uid, steering, xform, targetDistance);
         }
     }
 
@@ -253,7 +294,7 @@ public sealed partial class NPCSteeringSystem
             if (!node.Data.IsFreeSpace)
                 break;
 
-            var nodeMap = node.Coordinates.ToMap(EntityManager);
+            var nodeMap = node.Coordinates.ToMap(EntityManager, _transform);
 
             // If any nodes are 'behind us' relative to the target we'll prune them.
             // This isn't perfect but should fix most cases of stutter stepping.
@@ -311,7 +352,6 @@ public sealed partial class NPCSteeringSystem
         Angle offsetRot,
         Vector2 worldPos,
         float agentRadius,
-        float moveSpeed,
         int layer,
         int mask,
         TransformComponent xform,
@@ -414,7 +454,7 @@ public sealed partial class NPCSteeringSystem
 
             var xformB = xformQuery.GetComponent(ent);
 
-            if (!_physics.TryGetNearestPoints(uid, ent, out var pointA, out var pointB, xform, xformB))
+            if (!_physics.TryGetNearestPoints(uid, ent, out _, out var pointB, xform, xformB))
             {
                 continue;
             }

Content.Server/NPC/Systems/NPCSteeringSystem.cs

@@ -12,7 +12,6 @@ using Content.Shared.Movement.Components;
 using Content.Shared.Movement.Systems;
 using Content.Shared.NPC;
 using Content.Shared.NPC.Events;
-using Content.Shared.Physics;
 using Content.Shared.Weapons.Melee;
 using Robust.Server.Player;
 using Robust.Shared.Configuration;
@@ -49,15 +48,12 @@ namespace Content.Server.NPC.Systems
         [Dependency] private readonly DoorSystem _doors = default!;
         [Dependency] private readonly EntityLookupSystem _lookup = default!;
         [Dependency] private readonly FactionSystem _faction = default!;
-        // [Dependency] private readonly MetaDataSystem _metadata = default!;
         [Dependency] private readonly PathfindingSystem _pathfindingSystem = default!;
         [Dependency] private readonly SharedInteractionSystem _interaction = default!;
         [Dependency] private readonly SharedMeleeWeaponSystem _melee = default!;
         [Dependency] private readonly SharedMoverController _mover = default!;
         [Dependency] private readonly SharedPhysicsSystem _physics = default!;
-
+        [Dependency] private readonly SharedTransformSystem _transform = default!;
-        // This will likely get moved onto an abstract pathfinding node that specifies the max distance allowed from the coordinate.
-        private const float TileTolerance = 0.40f;
 
         private bool _enabled;
 
@@ -69,9 +65,17 @@ namespace Content.Server.NPC.Systems
 
         private object _obstacles = new();
 
+        private ISawmill _sawmill = default!;
+
         public override void Initialize()
         {
             base.Initialize();
+            _sawmill = Logger.GetSawmill("npc.steering");
+#if DEBUG
+            _sawmill.Level = LogLevel.Warning;
+#else
+            _sawmill.Level = LogLevel.Debug;
+#endif
 
             for (var i = 0; i < InterestDirections; i++)
             {
@@ -83,6 +87,7 @@ namespace Content.Server.NPC.Systems
             _configManager.OnValueChanged(CCVars.NPCPathfinding, SetNPCPathfinding, true);
 
             SubscribeLocalEvent<NPCSteeringComponent, ComponentShutdown>(OnSteeringShutdown);
+            SubscribeLocalEvent<NPCSteeringComponent, EntityUnpausedEvent>(OnSteeringUnpaused);
             SubscribeNetworkEvent<RequestNPCSteeringDebugEvent>(OnDebugRequest);
         }
 
@@ -140,6 +145,12 @@ namespace Content.Server.NPC.Systems
             component.PathfindToken = null;
         }
 
+        private void OnSteeringUnpaused(EntityUid uid, NPCSteeringComponent component, ref EntityUnpausedEvent args)
+        {
+            component.LastStuckTime += args.PausedTime;
+            component.NextSteer += args.PausedTime;
+        }
+
         /// <summary>
         /// Adds the AI to the steering system to move towards a specific target
         /// </summary>
@@ -157,6 +168,7 @@ namespace Content.Server.NPC.Systems
                 component.Flags = _pathfindingSystem.GetFlags(uid);
             }
 
+            ResetStuck(component, Transform(uid).Coordinates);
             component.Coordinates = coordinates;
             return component;
         }
@@ -183,7 +195,7 @@ namespace Content.Server.NPC.Systems
             if (!Resolve(uid, ref component, false))
                 return;
 
-            if (EntityManager.TryGetComponent(component.Owner, out InputMoverComponent? controller))
+            if (EntityManager.TryGetComponent(uid, out InputMoverComponent? controller))
             {
                 controller.CurTickSprintMovement = Vector2.Zero;
             }
@@ -206,7 +218,7 @@ namespace Content.Server.NPC.Systems
             var xformQuery = GetEntityQuery<TransformComponent>();
 
             var npcs = EntityQuery<ActiveNPCComponent, NPCSteeringComponent, InputMoverComponent, TransformComponent>()
-                .ToArray();
+                .Select(o => (o.Item1.Owner, o.Item2, o.Item3, o.Item4)).ToArray();
 
             // Dependency issues across threads.
             var options = new ParallelOptions
@@ -217,9 +229,8 @@ namespace Content.Server.NPC.Systems
 
             Parallel.For(0, npcs.Length, options, i =>
             {
-                var (_, steering, mover, xform) = npcs[i];
+                var (uid, steering, mover, xform) = npcs[i];
-
+                Steer(uid, steering, mover, xform, modifierQuery, bodyQuery, xformQuery, frameTime, curTime);
-                Steer(steering, mover, xform, modifierQuery, bodyQuery, xformQuery, frameTime, curTime);
             });
 
 
@@ -227,10 +238,10 @@ namespace Content.Server.NPC.Systems
             {
                 var data = new List<NPCSteeringDebugData>(npcs.Length);
 
-                foreach (var (_, steering, mover, _) in npcs)
+                foreach (var (uid, steering, mover, _) in npcs)
                 {
                     data.Add(new NPCSteeringDebugData(
-                        mover.Owner,
+                        uid,
                         mover.CurTickSprintMovement,
                         steering.Interest,
                         steering.Danger,
@@ -260,6 +271,7 @@ namespace Content.Server.NPC.Systems
         /// Go through each steerer and combine their vectors
         /// </summary>
         private void Steer(
+            EntityUid uid,
             NPCSteeringComponent steering,
             InputMoverComponent mover,
             TransformComponent xform,
@@ -291,27 +303,16 @@ namespace Content.Server.NPC.Systems
                 return;
             }
 
-            /* If you wish to not steer every tick A) Add pause support B) fix overshoots to prevent dancing
-            var nextSteer = steering.LastTimeSteer + TimeSpan.FromSeconds(1f / NPCSteeringComponent.SteerFrequency);
-
-            if (nextSteer > _timing.CurTime)
-            {
-                SetDirection(mover, steering, steering.LastSteer, false);
-                return;
-            }
-            */
-
-            var uid = mover.Owner;
             var interest = steering.Interest;
             var danger = steering.Danger;
             var agentRadius = steering.Radius;
-            var worldPos = xform.WorldPosition;
+            var worldPos = _transform.GetWorldPosition(xform, xformQuery);
             var (layer, mask) = _physics.GetHardCollision(uid);
 
             // Use rotation relative to parent to rotate our context vectors by.
             var offsetRot = -_mover.GetParentGridAngle(mover);
             modifierQuery.TryGetComponent(uid, out var modifier);
-            var moveSpeed = GetSprintSpeed(steering.Owner, modifier);
+            var moveSpeed = GetSprintSpeed(uid, modifier);
             var body = bodyQuery.GetComponent(uid);
             var dangerPoints = steering.DangerPoints;
             dangerPoints.Clear();
@@ -324,8 +325,10 @@ namespace Content.Server.NPC.Systems
 
             var ev = new NPCSteeringEvent(steering, interest, danger, agentRadius, offsetRot, worldPos);
             RaiseLocalEvent(uid, ref ev);
+            // If seek has arrived at the target node for example then immediately re-steer.
+            var forceSteer = true;
 
-            if (steering.CanSeek && !TrySeek(uid, mover, steering, body, xform, offsetRot, moveSpeed, interest, bodyQuery,  frameTime))
+            if (steering.CanSeek && !TrySeek(uid, mover, steering, body, xform, offsetRot, moveSpeed, interest, bodyQuery, frameTime, ref forceSteer))
             {
                 SetDirection(mover, steering, Vector2.Zero);
                 return;
@@ -333,7 +336,7 @@ namespace Content.Server.NPC.Systems
             DebugTools.Assert(!float.IsNaN(interest[0]));
 
             // Avoid static objects like walls
-            CollisionAvoidance(uid, offsetRot, worldPos, agentRadius, moveSpeed, layer, mask, xform, danger, dangerPoints, bodyQuery, xformQuery);
+            CollisionAvoidance(uid, offsetRot, worldPos, agentRadius, layer, mask, xform, danger, dangerPoints, bodyQuery, xformQuery);
             DebugTools.Assert(!float.IsNaN(danger[0]));
 
             Separation(uid, offsetRot, worldPos, agentRadius, layer, mask, body, xform, danger, bodyQuery, xformQuery);
@@ -365,7 +368,7 @@ namespace Content.Server.NPC.Systems
             // I think doing this after all the ops above is best?
             // Originally I had it way above but sometimes mobs would overshoot their tile targets.
 
-            if (steering.NextSteer > curTime)
+            if (!forceSteer && steering.NextSteer > curTime)
             {
                 SetDirection(mover, steering, steering.LastSteerDirection, false);
                 return;
@@ -388,7 +391,7 @@ namespace Content.Server.NPC.Systems
         /// <summary>
         /// Get a new job from the pathfindingsystem
         /// </summary>
-        private async void RequestPath(NPCSteeringComponent steering, TransformComponent xform, float targetDistance)
+        private async void RequestPath(EntityUid uid, NPCSteeringComponent steering, TransformComponent xform, float targetDistance)
         {
             // If we already have a pathfinding request then don't grab another.
             // If we're in range then just beeline them; this can avoid stutter stepping and is an easy way to look nicer.
@@ -401,7 +404,7 @@ namespace Content.Server.NPC.Systems
             // If this still causes issues future sloth adjust the collision mask.
             if (targetPoly != null &&
                 steering.Coordinates.Position.Equals(Vector2.Zero) &&
-                _interaction.InRangeUnobstructed(steering.Owner, steering.Coordinates.EntityId, range: 30f))
+                _interaction.InRangeUnobstructed(uid, steering.Coordinates.EntityId, range: 30f))
             {
                 steering.CurrentPath.Clear();
                 steering.CurrentPath.Enqueue(targetPoly);
@@ -410,10 +413,10 @@ namespace Content.Server.NPC.Systems
 
             steering.PathfindToken = new CancellationTokenSource();
 
-            var flags = _pathfindingSystem.GetFlags(steering.Owner);
+            var flags = _pathfindingSystem.GetFlags(uid);
 
             var result = await _pathfindingSystem.GetPathSafe(
-                steering.Owner,
+                uid,
                 xform.Coordinates,
                 steering.Coordinates,
                 steering.Range,
@@ -435,7 +438,7 @@ namespace Content.Server.NPC.Systems
                 return;
             }
 
-            var targetPos = steering.Coordinates.ToMap(EntityManager);
+            var targetPos = steering.Coordinates.ToMap(EntityManager, _transform);
             var ourPos = xform.MapPosition;
 
             PrunePath(ourPos, targetPos.Position - ourPos.Position, result.Path);