void OnlinePlanExpansionExecution::OnGoalNodeSucceeded(_In_ IOlcbpPlan::NodeID nodeId)
{
GoalEx* pGoal = (GoalEx*)m_pOlcbpPlan->GetNode(nodeId);
_ASSERTE(pGoal->State() == ESTATE_Succeeded);
if (GetNodeData(nodeId).BelongingCase != nullptr)
{
LogInfo("Goal=%s succeeded, revising its case and retaining it", pGoal->ToString().c_str());
CaseEx* currentCase = GetLastCaseForGoalNode(nodeId);
m_pCbReasoner->Reviser()->Revise(currentCase, true);
//UpdateHistory(currentCase);
}
else
{
LogInfo("Goal=%s succeeded without expansion, no case to revise", pGoal->ToString().c_str());
}
UpdateBelongingSubplanChildrenWithParentReadiness(nodeId);
OnNodeDone(nodeId);
if (m_planRootNodeId == nodeId)
{
g_MessagePump->Send(new Message(0, MSG_PlanComplete));
}
}
void OnlinePlanExpansionExecution::OnGoalNodeFailed(_In_ IOlcbpPlan::NodeID nodeId)
{
GoalEx* pGoal = (GoalEx*)m_pOlcbpPlan->GetNode(nodeId);
_ASSERTE(pGoal->State() == ESTATE_Failed);
if (GetNodeData(nodeId).BelongingCase != nullptr)
{
LogInfo("Goal=%s failed, revising and retaining it", pGoal->ToString().c_str());
CaseEx* currentCase = GetLastCaseForGoalNode(nodeId);
m_pCbReasoner->Reviser()->Revise(currentCase, false);
//UpdateHistory(currentCase);
}
OnNodeDone(nodeId);
}
bool OnlinePlanExpansionExecution::UpdateGoalNode(_In_ IOlcbpPlan::NodeID currentNode, _In_ const WorldClock& clock)
{
bool expanded = false;
GoalEx* pCurrentGoalNode = (GoalEx*)m_pOlcbpPlan->GetNode(currentNode);
// fast return if node state reached a final state (i.e succeeded or failed)
_ASSERTE(!IsNodeDone(currentNode));
#pragma region Node Open
if (IsNodeOpen(currentNode))
{
_ASSERTE(pCurrentGoalNode->GetState() == ESTATE_NotPrepared);
_ASSERTE(IsNodeDone(currentNode) == false);
bool hasPreviousPlan = DestroyGoalSnippetIfExist(currentNode);
// The goal was previously expanded with a plan, but it somehow failed
// Thats why it is now open
// Revise the node belonging case as failed case
if (hasPreviousPlan)
{
LogInfo("Node with plan-step '%s' is open and has children nodes, case is sent for revision and children have been destroyed", pCurrentGoalNode->ToString().c_str());
CaseEx* currentCase = GetLastCaseForGoalNode(currentNode);
m_pCbReasoner->Reviser()->Revise(currentCase, false);
UpdateHistory(currentCase);
}
if (pCurrentGoalNode->SuccessConditionsSatisfied(*g_Game))
{
LogInfo("Goal %s already satisfied, no need to expand it, closing the node", pCurrentGoalNode->ToString().c_str());
pCurrentGoalNode->SetState(ESTATE_Succeeded, *g_Game, clock);
CloseNode(currentNode);
OnGoalNodeSucceeded(currentNode);
}
else
{
IOlcbpPlan::NodeID satisfyingGoalNode = GetNodeData(currentNode).SatisfyingGoal;
AbstractRetriever::RetrieveOptions options;
options.ExcludedCases = GetNodeData(currentNode).TriedCases;
if (satisfyingGoalNode != IOlcbpPlan::NullNodeID)
{
LogInfo("Excluding satisfying goal node %s to avoid recursive plan expansion", m_pOlcbpPlan->GetNode(satisfyingGoalNode)->ToString().c_str());
// Add belonging case to exclusion to avoid recursive expansion of plans
_ASSERTE(GetNodeData(satisfyingGoalNode).BelongingCase != nullptr);
options.ExcludedGoalHashes.insert(GetNodeData(satisfyingGoalNode).BelongingCase->Goal()->Hash());
}
//pCurrentGoalNode->AdaptParameters(*g_Game);
options.GoalTypeId = (GoalType)pCurrentGoalNode->StepTypeId();
options.pGameState = g_Game;
options.Parameters = pCurrentGoalNode->Parameters();
CaseEx* pCandidateCase = m_pCbReasoner->Retriever()->Retrieve(options);
// We found a matching case and it was not tried for that goal before
if (pCandidateCase != nullptr)
{
// Retriever should always retrieve a non tried case for that specific node
// _ASSERTE(!IsCaseTried(currentNode, pCandidateCase));
LogInfo("Retrieved case '%s' has not been tried before, and its goal is being sent for expansion",
pCandidateCase->Goal()->ToString().c_str());
MarkCaseAsTried(currentNode, pCandidateCase);
ExpandGoal(currentNode, pCandidateCase);
expanded = true;
}
else
{
// The current failed goal node is the root goal and the planner exhausted all possible
// plans form the case-base for that goal node and nothing succeeded so far
//
// WE SURRENDER!!
//
if (m_planRootNodeId == currentNode)
{
LogWarning("Planner has exhausted all possible cases, WE SURRENDER!!");
m_planRootNodeId = IOlcbpPlan::NullNodeID;
}
else
{
LogInfo("Goal=%s exhausted all possible cases, failing it", pCurrentGoalNode->ToString().c_str());
pCurrentGoalNode->SetState(ESTATE_Failed, *g_Game, clock);
CloseNode(currentNode);
OnGoalNodeFailed(currentNode);
}
}
}
}
#pragma endregion
#pragma region Node Closed
else
{
_ASSERTE(pCurrentGoalNode->GetState() == ESTATE_NotPrepared);
if (pCurrentGoalNode->SuccessConditionsSatisfied(*g_Game)
&& !HasActiveAction(currentNode))
{
pCurrentGoalNode->SetState(ESTATE_Succeeded, *g_Game, clock);
OnGoalNodeSucceeded(currentNode);
}
else
{
// The goal is not done yet, and all of its children are done and
// finished execution. It does not make sense for the goal to continue
// this goal should fail, it has no future
if (GetNodeData(currentNode).WaitOnChildrenCount == 0)
{
if (!pCurrentGoalNode->IsSleeping(clock))
{
if (pCurrentGoalNode->SleepsCount() < GoalMaxSleepsCount)
{
LogInfo("%s is still not done and all of its children are done execution, slept %d time(s) before, will send it to sleep", pCurrentGoalNode->ToString().c_str(), pCurrentGoalNode->SleepsCount());
pCurrentGoalNode->Sleep(clock, GoalSleepTime);
}
else
{
LogInfo("%s is still not done and all of its children are done execution, already tried to sleep it %d time(s) before, failing it", pCurrentGoalNode->ToString().c_str(), GoalMaxSleepsCount);
OpenNode(currentNode);
}
}
}
}
}
#pragma endregion
return expanded;
}
//--------------------------------------------------------------------------------------------------------------
bool IStrategizer::LearningFromHumanDemonstration::SubplansDetection(vector<CookedPlan*>& p_cookedPlans) const
{
SubplansMap subplans;
// Detecting the subplans and the matched nodes
for (unsigned i = 0; i < p_cookedPlans.size(); ++i)
{
for (int j = i - 1; j >= 0; --j)
{
LogInfo("Checking if %d is supergraph of %d", i, j);
OlcbpPlan::NodeSet matchedIds;
if (IsSuperGraph(p_cookedPlans[i]->pPlan, p_cookedPlans[j]->pPlan, matchedIds))
{
subplans[i].push_back(SubsetPlanData(j, matchedIds));
}
}
}
if (subplans.empty())
{
return false;
}
for (unsigned i = 0; i < p_cookedPlans.size(); ++i)
{
// Remove duplicated nodes between subplans
for (unsigned j = 0; j < subplans[i].size(); ++j)
{
for (unsigned k = j + 1; k < subplans[i].size(); ++k)
{
for (OlcbpPlan::NodeID nodeId : subplans[i][j].Nodes)
{
subplans[i][k].Nodes.erase(nodeId);
}
}
}
for (SubsetPlanData subplanData : subplans[i])
{
if (subplanData.Nodes.empty())
{
continue;
}
unsigned subplanInx = subplanData.Index;
unsigned subplanId = p_cookedPlans[subplanInx]->Goal->Id();
unsigned nodeOrder = subplanId;
OlcbpPlan::NodeSet matchedIds = subplanData.Nodes;
GoalEx* pGoalClone = (GoalEx*)p_cookedPlans[subplanInx]->Goal->Clone();
pGoalClone->Id(subplanId);
if (p_cookedPlans[i]->pPlan->Contains(subplanId))
{
subplanId = PlanStepEx::GenerateID();
pGoalClone->Id(subplanId);
}
// Substitute the detected subplans in the super plan
_ASSERTE(subplanId == pGoalClone->Id());
p_cookedPlans[i]->pPlan->SubGraphSubstitution(matchedIds, pGoalClone, subplanId, nodeOrder);
LogInfo("Subistituting %d/%d", i, subplanInx);
}
}
return true;
}
void OnlinePlanExpansionExecution::CoverFailedGoals()
{
IOlcbpPlan::NodeQueue Q;
IOlcbpPlan::NodeID currNodeId;
IOlcbpPlan::NodeSerializedSet planRoots;
IOlcbpPlan::NodeSet visitedNodes;
IOlcbpPlan::NodeSet toCover;
_ASSERTE(m_planRootNodeId != IOlcbpPlan::NullNodeID);
Q.push(m_planRootNodeId);
visitedNodes.insert(m_planRootNodeId);
while (!Q.empty())
{
currNodeId = Q.front();
Q.pop();
if (!m_pOlcbpPlan->Contains(currNodeId))
{
LogWarning("A non existing node was there in the update queue, skipping it");
continue;
}
for (auto childNodeId : m_pOlcbpPlan->GetAdjacentNodes(currNodeId))
{
if (visitedNodes.count(childNodeId) == 0)
{
if (IsGoalNode(childNodeId) && IsNodeDone(childNodeId))
{
auto goalNode = m_pOlcbpPlan->GetNode(childNodeId);
if (m_coverGoals.count(childNodeId) == 0 &&
!goalNode->SuccessConditionsSatisfied(*g_Game) &&
m_backupNodes.count(childNodeId) == 0)
{
m_coverGoals.insert(childNodeId);
toCover.insert(childNodeId);
}
}
visitedNodes.insert(childNodeId);
Q.push(childNodeId);
}
}
}
for (auto nodeId : toCover)
{
auto goalNode = m_pOlcbpPlan->GetNode(nodeId);
GoalEx* backupNode = (GoalEx*)goalNode->Clone();
m_backupNodes.insert(MakePair(backupNode->Id(), nodeId));
backupNode->SetState(ESTATE_NotPrepared);
m_pOlcbpPlan->AddNode(backupNode, backupNode->Id());
m_nodeData[backupNode->Id()] = OlcbpPlanNodeData();
m_nodeData[backupNode->Id()].ID = backupNode->Id();
SetNodeSatisfyingGoal(backupNode->Id(), m_planRootNodeId);
LinkNodes(m_planRootNodeId, backupNode->Id());
auto& rootNodeData = GetNodeData(m_planRootNodeId);
rootNodeData.SetWaitOnChildrenCount(rootNodeData.WaitOnChildrenCount + 1);
}
}
void LfhdCbNodeSelector::Select(_Out_ IOlcbpPlan::NodeSet& goalsToUpdate,
_Out_ IOlcbpPlan::NodeDQueue& actionsToUpdate,
_Out_ IOlcbpPlan::NodeSet& snippetsToDestroy)
{
IOlcbpPlan* pPlan = m_pPlanner->Plan();
IOlcbpPlan::NodeQueue actionQ;
IOlcbpPlan::NodeQueue goalQ;
typedef unsigned GoalKey;
map<GoalKey, stack<IOlcbpPlan::NodeID>> goalTable;
// 1st pass: get ready nodes only
m_pPlanner->GetReachableReadyNodes(actionQ, goalQ);
while (!goalQ.empty())
{
IOlcbpPlan::NodeID currentGoalId = goalQ.front();
goalQ.pop();
if (!pPlan->Contains(currentGoalId))
continue;
GoalEx* pCurrGoal = (GoalEx*)pPlan->GetNode(currentGoalId);
GoalKey typeKey = pCurrGoal->Key();
if (!m_pPlanner->IsNodeDone(currentGoalId))
{
IOlcbpPlan::NodeID newActiveGoalId = currentGoalId;
if (goalTable.count(typeKey) == 0)
goalTable[typeKey].push(newActiveGoalId);
else
{
auto& currentOverrideStack = goalTable[typeKey];
IOlcbpPlan::NodeID currentActiveGoalId = currentOverrideStack.top();
IOlcbpPlan::NodeSet ancestors;
stack<IOlcbpPlan::NodeID> newOverrideStack;
m_pPlanner->GetAncestorSatisfyingGoals(newActiveGoalId, ancestors);
newOverrideStack.push(newActiveGoalId);
while (!currentOverrideStack.empty())
{
currentActiveGoalId = currentOverrideStack.top();
// Nodes belonging to my ancestors set should not be destroyed
// and should be kept in order
if (ancestors.count(currentActiveGoalId) != 0)
{
newOverrideStack.push(currentActiveGoalId);
}
else if (pPlan->Contains(currentActiveGoalId) &&
!m_pPlanner->IsNodeOpen(currentActiveGoalId) &&
!m_pPlanner->HasActiveAction(currentActiveGoalId))
{
snippetsToDestroy.insert(currentActiveGoalId);
}
currentOverrideStack.pop();
}
// Push back new stack into the current one in a LIFO order to reverse the original one
while (!newOverrideStack.empty())
{
currentOverrideStack.push(newOverrideStack.top());
newOverrideStack.pop();
}
}
}
}
for (auto& r : goalTable)
{
goalsToUpdate.insert(r.second.top());
}
while (!actionQ.empty())
{
auto actionNodeId = actionQ.front();
bool satisfyingGoalActive = goalsToUpdate.count(m_pPlanner->GetNodeData(actionNodeId).SatisfyingGoal) > 0;
bool satisfyingGoalDone = m_pPlanner->IsNodeDone(m_pPlanner->GetNodeData(actionNodeId).SatisfyingGoal);
if (satisfyingGoalActive || satisfyingGoalDone)
{
actionsToUpdate.push_back(actionNodeId);
}
actionQ.pop();
}
}
