TurnTowardsTypeNode::TurnTowardsTypeNode() {
//Search and turn for the current Target
SequenceNode *cSequence = new SequenceNode();
cSequence->addChild(new SearchNode());
cSequence->addChild(new InverterNode(new IsTurnedOnTargetNode(1)));
cSequence->addChild(new TurnNode());
cSequence->addChild(new TurnNeckToCenterNode());
cSequence->addChild(new WaitSeeStateUpdateNode());
cSequence->addChild(new WaitBodyStateUpdateNode());
cSequence->addChild(new EndActNode());
addChild(new SuccessNode(new RepeatUntilFailNode(cSequence)));
addChild(new SuccessNode(new PushTargetToStackNode()));
addChild(new SetTargetToNode(new BehaviorTarget(WidthTypeNarrow, QualityTypeHigh, NODE_NAME)));
addChild(new ChangeViewNode());
addChild(new SuccessNode(new PopFromStackNode()));
//Update variables and body state
addChild(new EndActNode());
}
SearchNode::SearchNode() : InverterNode(new SequenceNode()) {
//Take the existing child into a pointer
SequenceNode *cCheckingSequence = static_cast<SequenceNode*>(&getChild());
/********************************************
* Checks if the target is visible, if not *
* changes the view settings for search. *
*******************************************/
/*
* First check if the object is already visible.
* if not continue the sequence, otherwise
* stop the sequence once found. For the sequence to
* continue, a successful answer must be received from
* not seeing the target. We will use an Inverter.
*/
InverterNode *cInvert = new InverterNode(new IsTargetVisibleNode());
cCheckingSequence->addChild(cInvert);
/*
* When searching, first we need to change the view quality to low,
* and the width angle to wide. Because we are searching
* for a specific target in context, we save it onto
* the stack, load our target, pop the stack and continue.
*/
//Push the current target to the stack - dont care if there isnt a target
cCheckingSequence->addChild(new SuccessNode(new PushTargetToStackNode()));
//Create our required target for view changing
cCheckingSequence->addChild(new SetTargetToNode(new BehaviorTarget(WidthTypeWide, QualityTypeLow, NODE_NAME)));
//Change the view
cCheckingSequence->addChild(new ChangeViewNode());
//Pop the previous target back to the context - dont care if there isnt a target
cCheckingSequence->addChild(new SuccessNode(new PopFromStackNode()));
//Call the EndAct node to update the view settings
cCheckingSequence->addChild(new ExecuteActNode());
/*
* This next section will be repeated
* until the target is found.
*/
SequenceNode *cSequence = new SequenceNode();
/********************************************
* Rotate the head - check center, *
* left and right. *
*******************************************/
cSequence->addChild(new InverterNode(new SearchTurnNeckNode()));
/********************************************
* Turn the body, and check again. The *
* turning of the body is decided by *
* the maximum angle. *
*******************************************/
cSequence->addChild(new TurnTwiceMaximumHeadAngleNode());
//Implement the repeater
cCheckingSequence->addChild(new RepeatUntilFailNode(cSequence));
}
SearchTurnNeckNode::SearchTurnNeckNode() : InverterNode(new SequenceNode()) {
/************************************************
* Check if visibile on center, otherwise turn *
* to right, check again, and if not turn left. *
***********************************************/
//Take the existing child into a pointer
SequenceNode *cCheckingSequence = static_cast<SequenceNode*>(&getChild());
/*
* If not visible, we need to continue the sequence,
* thus we will use an Inverter. The addVisibilityCheck
* does this process for us.
*/
addVisibilityCheck(cCheckingSequence);
/*
* If not found, the sequence continues and we need to turn the neck.
* Need to repeat the positive rotation until maximum, at which it
* will fail. Thus we need to repeat until fail.
*/
TurnNeckNegativeNode *cTurnNegative = new TurnNeckNegativeNode();
//Also check visibility at each turn
addVisibilityCheck(cTurnNegative);
//Repeat the node until it fails
RepeatUntilFailNode *cRepeatUntilFailNeg = new RepeatUntilFailNode(cTurnNegative);
//Dont want to stop sequence here if found node or just cant proceed - it will always be true
SuccessNode *cSuccessRepeatNeg = new SuccessNode(cRepeatUntilFailNeg);
cCheckingSequence->addChild(cSuccessRepeatNeg);
/*
* At this section, we would have already turned the neck
* to the maximum angle. We need to check again if the
* target is visible using an inverter just like before.
*/
addVisibilityCheck(cCheckingSequence);
TurnNeckPositiveNode *cTurnPositive = new TurnNeckPositiveNode();
//Also check visibility at each turn
addVisibilityCheck(cTurnPositive);
//Repeat the node until it fails
RepeatUntilFailNode *cRepeatUntilFailPos = new RepeatUntilFailNode(cTurnPositive);
//Dont want to stop sequence here if found node or just cant proceed - it will always be true
SuccessNode *cSuccessRepeatPos = new SuccessNode(cRepeatUntilFailPos);
cCheckingSequence->addChild(cSuccessRepeatPos);
/*
* At this section, we would have already turned the neck
* to the minimum angle. We need to check again if the
* target is visible using an inverter just like before.
*/
addVisibilityCheck(cCheckingSequence);
/*
* If the sequence still continues, it means that we
* havnt found the target. Thus, turn the neck back to
* the center.
*/
//The result of this node should be disregarded, thus always succedes
cCheckingSequence->addChild(new SuccessNode(new TurnNeckToCenterNode()));
cCheckingSequence->addChild(new ExecuteActNode());
}
