/* 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; }