public CompleteCurrentTaskResult CompleteCurrentTask(IState state)
{
if (_currentTask != null)
{
if (!_currentTask.Terminate(state, true))
{
_currentPlan = null;
_currentTask = null;
return(CompleteCurrentTaskResult.PlanFailed);
}
_currentTask.Apply(state);
_currentTask = null;
}
if (_currentPlan == null)
{
return(CompleteCurrentTaskResult.NoPlan);
}
if (_currentTask == null && _currentPlan.Count == 0)
{
_currentPlan = null;
return(CompleteCurrentTaskResult.PlanSuccess);
}
_currentTask = _currentPlan.Pop();
if (!_currentTask.Check(state))
{
_currentPlan = null;
_currentTask = null;
return(CompleteCurrentTaskResult.PlanFailed);
}
if (!_currentTask.Execute(state))
{
_currentPlan = null;
_currentTask = null;
return(CompleteCurrentTaskResult.PlanFailed);
}
return(CompleteCurrentTaskResult.ContinuePlan);
}
public void Update(IState state, TaskBase rootTask)
{
Assert.IsNotNull(rootTask);
if (_currentPlan == null)
{
if (Time.time >= _minNextPlanTime)
{
_minNextPlanTime = Time.time + 0.25f;
var watch = System.Diagnostics.Stopwatch.StartNew();
// the code that you want to measure comes here
if (Planner.Plan(state, rootTask, out _currentPlan))
{
watch.Stop();
Debug.Log("Plan (took " + watch.ElapsedMilliseconds + "ms): " + _currentPlan.Aggregate("", (acc, task) => acc + task.name + ","));
CompleteCurrentTask(state);
}
}
}
}
public override object GetValue(NodePort port)
{
task = GetInputValue <TaskBase>("task");
return(this);
}
public override object GetValue(NodePort port)
{
task = this;
return(this);
}
public static bool Plan(IState currentState, TaskBase rootTask, out Stack <TaskBase> finalPlan)
{
// The setup
Assert.IsNotNull(currentState);
Assert.IsNotNull(rootTask);
var plannerState = new State()
{
tasksToProcess = new Stack <TaskBase>(),
finalPlan = new List <TaskBase>(),
workingWS = currentState.Clone()
};
plannerState.tasksToProcess.Push(rootTask);
plannerState.nextMethodIdx = 0;
// The heist
var decompHistory = new Stack <State>();
while (plannerState.tasksToProcess.Count > 0)
{
var currentTask = plannerState.tasksToProcess.Pop();
if (currentTask is CompoundTask currentTaskCompound)
{
//Debug.Log(Padd(decompHistory) + "CT " + currentTask.name);
if (plannerState.nextMethodPtr != currentTaskCompound)
{
plannerState.nextMethodPtr = currentTaskCompound;
plannerState.nextMethodIdx = 0;
}
var satisfiedMethod = FindSatisfiedMethod(currentTaskCompound, plannerState.workingWS, ref plannerState.nextMethodIdx);
if (satisfiedMethod != null)
{
RecordDecompositionOfTask(plannerState, currentTaskCompound, decompHistory);
for (int i = satisfiedMethod.tasks.Length - 1; i >= 0; --i)
{
var t = satisfiedMethod.tasks[i];
plannerState.tasksToProcess.Push(t);
}
}
else
{
RestoreToLastDecomposedTask(ref plannerState, decompHistory, currentTaskCompound.name + " no satisfied method");
}
}
else //Primitive Task
//Debug.Log(Padd(decompHistory) + "PT " + currentTask.name);
{
if (currentTask.Check(plannerState.workingWS))
{
currentTask.Apply(plannerState.workingWS);
plannerState.finalPlan.Add(currentTask);
}
else
{
RestoreToLastDecomposedTask(ref plannerState, decompHistory, currentTask + " check failed");
}
}
}
// Final countdown
if (plannerState.finalPlan.Count == 0)
{
finalPlan = null;
return(false);
}
plannerState.finalPlan.Reverse();
finalPlan = new Stack <TaskBase>(plannerState.finalPlan);
//Debug.Log("=== DONE ===");
return(true);
}
