/*
Service call when we change the state of a goal/plan/action
*/
bool changeState(supervisor_msgs::ChangeState::Request &req, supervisor_msgs::ChangeState::Response &res){
if(req.type == "action"){
return actionState(req.action, req.state);
}else if(req.type == "plan"){
if(req.state == "PROGRESS"){
return newPlan(req.plan, false);
}else if(req.state == "SHARE"){
return sharePlan();
}else if(req.state == "PROGRESS_SHARE"){
return newPlan(req.plan, true);
}else if(req.state == "ABORT"){
return abortPlan(req.agent);
}
}else if(req.type == "goal"){
if(req.state == "NEW"){
return newGoal(req.goal);
}else if(req.state == "PROGRESS"){
return startGoal(req.goal);
}else if(req.state == "ABORT"){
return abortGoal(req.goal, req.agent);
}
}
}
/** IDA* Algorithm */
node *iterativeDeepeningAStar(node *Node, int bound, int *tempBound, long long *generatedNodes, long long *expandedNodes, int *prevBlankPosition)
{
/** Iterator */
int i;
/** Temporary Node and Result node*/
node *tempNode = malloc(sizeof(node));
node *result = malloc(sizeof(node));
/** Move list of possible actions */
int moveList[MAX_POSSIBLE_MOVES];
/** State after it has been moved */
int newState[SIZE];
/** Get the possible moves into move list */
generateMoves(Node, moveList);
/** IDA Star algorithm
* Loop through maximum actions possible
*/
for(i = 0; i < MAX_POSSIBLE_MOVES; i++)
{
/** Only perform valid moves */
if(moveList[i] == INVALID) {continue;}
/** A node was generated */
*generatedNodes = *generatedNodes + 1;
/** Update the state and store it in newState
* n'.s = f(a, n.s)
*/
tempNode->state = actionState(moveList[i], Node->state, newState, prevBlankPosition);
/** Increment the cost
* n'.g(n) = n.g(n) + c(a, n.s)
*/
tempNode->cost = Node->cost + COST_VALUE;
/** Update the threshold
* n'.f(n) = n'.g(n) + h(n'.s)
*/
tempNode->threshold = tempNode->cost + heuristicUnweightedValue(tempNode->state);
/* Check if the threshold is greater than the bound and set it to the minimum
* Chek if n'.f(n) > B
*/
if(tempNode->threshold > bound)
{
*tempBound = minimum(tempNode->threshold, *tempBound);
}
else
{
/* If we have reached our goal, return the node
* h(n'.s) := 0
*/
if(heuristicUnweightedValue(tempNode->state) == 0) {return tempNode;}
/** A node was expanded upon */
*expandedNodes = *expandedNodes + 1;
/* printf("Expanding: %d TempBound: %d\n", *expandedNodes, *tempBound); */
/** Recursively repeat */
result = iterativeDeepeningAStar(tempNode, bound, tempBound, generatedNodes, expandedNodes, prevBlankPosition);
if(result != NULL) {return result;}
}
}
/** Free the node */
free(tempNode);
return NULL;
}
