Merge pull request #9 from ptrefall/bugfix/issue-8

Fixed issue where MTR [0,0,1] wouldn't beat LastMTR [0,1,0] during a …
ptrefall · Apr 22, 2021 · d9006ce · d9006ce
2 parents de2c390 + 9d8f528
commit d9006ce
Show file tree

Hide file tree

Showing 2 changed files with 62 additions and 5 deletions.
diff --git a/Fluid-HTN.UnitTests/SelectorTests.cs b/Fluid-HTN.UnitTests/SelectorTests.cs
@@ -289,5 +289,36 @@ public void DecomposeCompoundSubtaskLoseToLastMTR_ExpectedBehavior()
             Assert.IsTrue(ctx.MethodTraversalRecord.Count == 1);
             Assert.IsTrue(ctx.MethodTraversalRecord[0] == -1);
         }
+
+        [TestMethod]
+        public void DecomposeCompoundSubtaskWinOverLastMTR_ExpectedBehavior()
+        {
+            var ctx = new MyContext();
+            var rootTask = new Selector() { Name = "Root" };
+            var task = new Selector() { Name = "Test1" };
+            var task2 = new Selector() { Name = "Test2" };
+            var task3 = new Selector() {Name = "Test3"};
+
+            task3.AddSubtask(new PrimitiveTask() { Name = "Sub-task3-1" }.AddCondition(new FuncCondition<MyContext>("Done == true", context => context.Done == true)));
+            task3.AddSubtask(new PrimitiveTask() { Name = "Sub-task3-2" });
+
+            task2.AddSubtask(new PrimitiveTask() { Name = "Sub-task2-1" }.AddCondition(new FuncCondition<MyContext>("Done == true", context => context.Done == true)));
+            task2.AddSubtask(new PrimitiveTask() { Name = "Sub-task2-2" });
+
+            task.AddSubtask(task2);
+            task.AddSubtask(task3);
+            task.AddSubtask(new PrimitiveTask() { Name = "Sub-task1-1" }.AddCondition(new FuncCondition<MyContext>("Done == false", context => context.Done == false)));
+
+            rootTask.AddSubtask(task);
+
+            ctx.LastMTR.Add(0);
+            ctx.LastMTR.Add(1);
+            ctx.LastMTR.Add(0);
+
+            // In this test, we prove that [0, 0, 1] beats [0, 1, 0]
+            var status = rootTask.Decompose(ctx, 0, out var plan);
+
+            Assert.IsTrue(status == DecompositionStatus.Succeeded);
+        }
     }
 }
diff --git a/Fluid-HTN/Tasks/CompoundTasks/Selector.cs b/Fluid-HTN/Tasks/CompoundTasks/Selector.cs
@@ -32,6 +32,36 @@ public override bool IsValid(IContext ctx)
             return true;
         }
 
+        private bool BeatsLastMTR(IContext ctx, int taskIndex, int currentDecompositionIndex)
+        {
+            // If the last plan's traversal record for this decomposition layer 
+            // has a smaller index than the current task index we're about to
+            // decompose, then the new decomposition can't possibly beat the
+            // running plan, so we cancel finding a new plan.
+            if (ctx.LastMTR[currentDecompositionIndex] < taskIndex)
+            {
+                // But, if any of the earlier records beat the record in LastMTR, we're still good, as we're on a higher priority branch.
+                // This ensures that [0,0,1] can beat [0,1,0]
+                for (var i = 0; i < ctx.MethodTraversalRecord.Count; i++)
+                {
+                    var diff = ctx.MethodTraversalRecord[i] - ctx.LastMTR[i];
+                    if (diff < 0)
+                    {
+                        return true;
+                    }
+                    if (diff > 0)
+                    {
+                        // We should never really be able to get here, but just in case.
+                        return false;
+                    }
+                }
+
+                return false;
+            }
+
+            return true;
+        }
+
         // ========================================================= DECOMPOSITION
 
         /// <summary>
@@ -53,12 +83,8 @@ protected override DecompositionStatus OnDecompose(IContext ctx, int startIndex,
                 {
                     if (ctx.MethodTraversalRecord.Count < ctx.LastMTR.Count)
                     {
-                        // If the last plan's traversal record for this decomposition layer 
-                        // has a smaller index than the current task index we're about to
-                        // decompose, then the new decomposition can't possibly beat the
-                        // running plan, so we cancel finding a new plan.
                         var currentDecompositionIndex = ctx.MethodTraversalRecord.Count;
-                        if (ctx.LastMTR[currentDecompositionIndex] < taskIndex)
+                        if (BeatsLastMTR(ctx, taskIndex, currentDecompositionIndex) == false)
                         {
                             ctx.MethodTraversalRecord.Add(-1);
                             if (ctx.DebugMTR) ctx.MTRDebug.Add($"REPLAN FAIL {Subtasks[taskIndex].Name}");