void OnlinePlanExpansionExecution::UpdateHistory(CaseEx* pCase)
{
IOlcbpPlan *pCasePlan = pCase->Plan();
IOlcbpPlan::NodeSerializedSet casePlanNodes = pCasePlan->GetNodes();
for (auto caseNodeId : casePlanNodes)
{
IOlcbpPlan::NodeValue pOriginalNode = pCasePlan->GetNode(caseNodeId);
if (BELONG(ActionType, pCasePlan->GetNode(caseNodeId)->StepTypeId()))
{
Action* pOriginalActionNode = (Action*)pOriginalNode;
Action* pClonedActionNode = (Action*)m_clonedNodesMapping[pOriginalNode];
pOriginalActionNode->SetExecutionHistory(pClonedActionNode->GetExecutionHistory());
}
}
}
void OnlinePlanExpansionExecution::ExpandGoal(_In_ IOlcbpPlan::NodeID expansionGoalNodeId, _In_ CaseEx *pExpansionCase)
{
LogActivity(ExpandGoal);
IOlcbpPlan *pCasePlan = pExpansionCase->Plan();
IOlcbpPlan::NodeSerializedSet casePlanRoots = pCasePlan->GetOrphanNodes();
IOlcbpPlan::NodeSerializedSet casePlanNodes = pCasePlan->GetNodes();
IOlcbpPlan::NodeSerializedSet newExpansionPlanRoots;
LogInfo("Expanding node[%d] with plan %s", expansionGoalNodeId, pExpansionCase->Plan()->ToString().c_str());
// 1. Construct planner plan graph nodes from the expansion case
SetGoalNodeBelongingCase(expansionGoalNodeId, pExpansionCase);
// Cross mapping between node IDs in in the planner plan and case plan
std::map<IOlcbpPlan::NodeID, IOlcbpPlan::NodeID> plannerToCasePlanNodeIdMap;
std::map<IOlcbpPlan::NodeID, IOlcbpPlan::NodeID> casePlanToPlannerNodeIdMap;
for(auto caseNodeId : casePlanNodes)
{
IOlcbpPlan::NodeValue pOriginalNode = pCasePlan->GetNode(caseNodeId);
IOlcbpPlan::NodeValue pNode = static_cast<PlanStepEx*>(const_cast<PlanStepEx*>(pOriginalNode)->Clone());
IOlcbpPlan::NodeID plannerNodeId = m_pOlcbpPlan->AddNode(pNode, pNode->Id());
m_nodeData[plannerNodeId] = OlcbpPlanNodeData();
m_nodeData[plannerNodeId].ID = plannerNodeId;
// Add Data record for the new node
SetNodeSatisfyingGoal(plannerNodeId, expansionGoalNodeId);
if (casePlanRoots.count(caseNodeId) > 0)
newExpansionPlanRoots.insert(plannerNodeId);
if (IsActionNode(plannerNodeId))
{
m_clonedNodesMapping[pOriginalNode] = pNode;
}
else if (IsGoalNode(plannerNodeId))
{
OpenNode(plannerNodeId);
}
// Map node ID in in the planner plan with its counterpart in case plan and vice versa
plannerToCasePlanNodeIdMap[plannerNodeId] = caseNodeId;
casePlanToPlannerNodeIdMap[caseNodeId] = plannerNodeId;
}
// 2. Link the goal node with the roots of the expansion plan
IOlcbpPlan::NodeQueue Q;
IOlcbpPlan::NodeSerializedSet visitedNodes;
for(auto rootNodeId : newExpansionPlanRoots)
{
// Cross link the goal node with the sub plan roots
LinkNodes(expansionGoalNodeId, rootNodeId);
// Enqueue the roots in a queue to expand them
Q.push(rootNodeId);
visitedNodes.insert(rootNodeId);
}
// 3. Continue construction of the sub plan tree with the rest of the plan graph nodes
while(!Q.empty())
{
IOlcbpPlan::NodeID currPlannerNodeId = Q.front();
Q.pop();
IOlcbpPlan::NodeSerializedSet currCaseChildren = pCasePlan->GetAdjacentNodes(plannerToCasePlanNodeIdMap[currPlannerNodeId]);
for (auto currCaseChildNodeId : currCaseChildren)
{
IOlcbpPlan::NodeID currPlannerChildNodeId = casePlanToPlannerNodeIdMap[currCaseChildNodeId];
// If my child not visited, consider it for expansion
if(!visitedNodes.count(currPlannerChildNodeId))
{
Q.push(currPlannerChildNodeId);
visitedNodes.insert(currPlannerChildNodeId);
}
// Cross link the current node with its child
LinkNodes(currPlannerNodeId, currPlannerChildNodeId);
}
}
int nodeAddRemoveDelta = 0; //AdaptSnippet(expansionGoalNodeId);
// Adaptation should leave at least 1 node in the snippet
_ASSERTE((int)pCasePlan->Size() + nodeAddRemoveDelta > 0);
size_t adaptedSnippetSize = (size_t)((int)pCasePlan->Size() + nodeAddRemoveDelta);
CloseNode(expansionGoalNodeId);
// 4. Since the plan structure changed, recompute the NotReadyParentsCount
// for plan graph node
ComputeFreshSnippetWaitOnParentsCount(expansionGoalNodeId);
// 5. Record that the expanded goal node is waiting for N number of open nodes
// If the number of those open nodes = 0, and the goal node is not satisfied, then
// there is no way for the goal to succeed, and the goal should fail
GetNodeData(expansionGoalNodeId).SetWaitOnChildrenCount(adaptedSnippetSize);
m_planStructureChangedThisFrame = true;
LogInfo("Plan after node[%d] expansion %s", expansionGoalNodeId, m_pOlcbpPlan->ToString().c_str());
}
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();
}
}
