void BehaviorNode::ApplyEffects(Agent* pAgent, BehaviorNode::EPhase phase) const
{
if (this->m_effectors.size() == 0)
{
return;
}
if (this->m_both_effectors == 0)
{
if (phase == Effector::E_SUCCESS && this->m_success_effectors == 0)
{
return;
}
if (phase == Effector::E_FAILURE && this->m_failure_effectors == 0)
{
return;
}
}
for (uint32_t i = 0; i < this->m_effectors.size(); ++i)
{
Effector* pEffector = (Effector*)this->m_effectors[i];
if (pEffector != NULL)
{
Effector::EPhase ph = pEffector->GetPhase();
if (phase == Effector::E_BOTH || ph == Effector::E_BOTH || ph == phase)
{
pEffector->Evaluate((Agent*)pAgent);
}
}
}
}
void Actor::loseEffectors(const EffectorIDVector& effectors, Action* parentAction)
{
//determine lost effectors for actions on effectors
ActionEffectorsInfo lostEffectorsInfo;
for (size_t i = 0; i<effectors.size(); i++)
{
Effector* effector = getEffector(effectors[i]);
GB_ASSERT(effector, format("Actor::getEffector: actor doesn't have effector %s", Effector::convertToString(effectors[i])));
Action* responsibleParent = getResponsibleParent(effectors[i],parentAction);
Actions activeActions;
effector->getActiveChildActions(responsibleParent, activeActions);
lostEffectorsInfo.addInfo(effector,activeActions);
}
//notify actions about lost effectors
ActionEffectorsInfo::ActionEffectorsDeque::reverse_iterator rit = lostEffectorsInfo.m_deque.rbegin(),
rend = lostEffectorsInfo.m_deque.rend();
while(rit!=rend)
{
Action* action = rit->action;
EffectorIDVector& effectors = rit->effectorIDVector;
action->loseEffectors(effectors);
rit++;
}
}
void Actor::updateEffectors()
{
GB_ASSERT(m_state,"Actor::updateEffectors: actor doesn't have state");
//delete actions
ActionSet::iterator ita = m_actionsToDelete.begin();
ActionSet::iterator enda = m_actionsToDelete.end();
while(ita!=enda)
{
Action* action = *ita;
GB_SAFE_DELETE(action);
ita++;
}
m_actionsToDelete.clear();
//update actions
Actions actionsToUpdate;
m_rootAction->getChilds(actionsToUpdate, true);
/*
for (Actions::reverse_iterator rit=actionsToUpdate.rbegin(), rend = actionsToUpdate.rend();
rit!=rend; rit++)
*/
for (Actions::iterator it=actionsToUpdate.begin(), end = actionsToUpdate.end();
it!=end; it++)
{
Action* action = *it; //*rit;
//check whether action wasn't terminated yet
if (action->getTerminationStatus()==Action::NONE)
{
action->update();
}
}
//notify actions about acquiring(activating) effectors
for (ActivatedEffectorsMap::iterator it = m_activatedEffectors.begin(),end = m_activatedEffectors.end();
it!=end; it++)
{
Action* action = it->first;
EffectorIDVector& effectors = it->second;
//check action activity on effectors
EffectorIDVector::iterator jt = effectors.begin();
while(jt!=effectors.end())
{
Effector* effector = getEffector(*jt);
if (effector->isActive(action))
jt++;
else
jt = effectors.erase(jt);
}
if (!effectors.empty())
{
//GB_INFO("Actor::update: reacquiring %s (%p)",action->getDescString().c_str(), action);
action->acquireEffectors(effectors);
}
}
m_activatedEffectors.clear();
}
void Actor::setActionToEffectors(Action* action, Action* parent)
{
GB_ASSERT(action,"Actor::setActionToEffectors: action must be defined");
EffectorIDVector actualNonAcquiredEffectors = action->getAcquiredEffectors();
const EffectorIDVector &requiredEffectors = action->getRequiredEffectors();
//set new action to effectors
for (size_t i = 0; i<requiredEffectors.size(); i++)
{
Effector* effector = getEffector(requiredEffectors[i]);
GB_ASSERT(effector, format("Actor::getEffector: actor doesn't have effector %s", \
Effector::convertToString(requiredEffectors[i])));
Actions parentChain;
getResponsibleParent(requiredEffectors[i],parent,&parentChain);
//insert parents
int customPriority = action->getEffectorPriority(requiredEffectors[i]);
//case of multiple responsible children
if (parentChain.size()==1 && (parent->hasEffectorResponsibleChild(requiredEffectors[i])))
{
parent->addResponsibleChildForEffector(requiredEffectors[i], action);
}
for (size_t j = parentChain.size()-1; j>0; j--)
{
//action notification about new added effectors can be placed here
Action* actionToInsert = parentChain[j-1];
Action* parentAction = parentChain[j];
Action* responsibleChild = j>1 ? parentChain[j-2] : action;
actionToInsert->addRequiredEffector(requiredEffectors[i],false,customPriority,responsibleChild);
if (effector->insertAction(actionToInsert,parentAction))
{
actionToInsert->addAcquiredEffector(requiredEffectors[i]);
}
}
//insert action
if (effector->insertAction(action,parent))
{
//maybe this is only for debug
EffectorIDVector::iterator it = std::find(actualNonAcquiredEffectors.begin(),actualNonAcquiredEffectors.end(),
requiredEffectors[i]);
GB_ASSERT(it!=actualNonAcquiredEffectors.end(),"Actor::setActionToEffectors: can't set action properly.");
actualNonAcquiredEffectors.erase(it);
}
}
GB_ASSERT(actualNonAcquiredEffectors.empty(),"Actor::setActionToEffectors: can't set action properly.");
}
Action* Actor::getActiveAction(const std::string actionName, const EffectorIDVector& effectors )
{
const EffectorIDVector& checkedEffectors = effectors.empty() ? m_effectorIDs : effectors;
EffectorIDVector::const_iterator it = checkedEffectors.begin();
EffectorIDVector::const_iterator end = checkedEffectors.end();
Action* action = NULL;
while (it!=end && !action)
{
Effector* effector = getEffector(*it);
action = effector->getActiveAction(actionName);
it++;
}
return action;
}
void Actor::printActions(std::ostringstream& buff, bool full)
{
buff.clear();
buff<<"Actor state: "<<m_state->getName()<<std::endl;
buff<<"Actor actions on effectors: "<<std::endl;
for(EffectorMap::iterator it = m_effectors.begin(),end = m_effectors.end();
it!=end;it++)
{
Effector* effector = it->second;
buff<<Effector::convertToString(effector->getId())<<std::endl;
std::string tabs;
print(effector,m_rootAction,buff,tabs,full);
}
}
bool Actor::checkEffectors(Action* action, Action* parentAction,EffectorIDVector &acquiredEffectors)
{
acquiredEffectors.clear();
bool principalAcquired = true;
const Action::EffectorInfoMap &requiredEffectors = action->getRequiredEffectorInfos();
for (Action::EffectorInfoMap::const_iterator it = requiredEffectors.begin(),end = requiredEffectors.end();
it!=end; it++)
{
EffectorID effectorID = it->first;
Effector* effector = getEffector(effectorID);
GB_ASSERT(effector, format("Actor::getEffector: actor doesn't have effector %s", Effector::convertToString(effectorID)));
bool principal = it->second.principal;
//check if effector will be acquired by action when action is setting to parent
bool isAcquired = false;
Actions parentChain;
Action* responsibleParent = getResponsibleParent(effectorID,parentAction,&parentChain);
if (effector->isActive(responsibleParent))
{
isAcquired = true;
int customActionPriority = action->getEffectorPriority(effectorID);
Action* currentAction = parentChain.size()>1 ? parentChain[parentChain.size()-2] : action;
Action* settedAction = effector->getActiveChildAction(responsibleParent);
//in our case, this can be done simpler, because comparing is based on action's priority
//and doesn't depend on action(so action can be used instead of currentAction)
if (!settedAction || compareActions(settedAction,currentAction,effectorID,
settedAction->getEffectorPriority(effectorID),customActionPriority))
{
isAcquired = true;
acquiredEffectors.push_back(effectorID);
}
}
if (principal && !isAcquired)
{
principalAcquired = false;
}
}
return principalAcquired;
}
void Actor::releaseActionEffectors(Action* action,const EffectorIDVector& effectorIDs)
{
for (size_t i=0;i<effectorIDs.size();i++)
{
Effector* effector = getEffector(effectorIDs[i]);
GB_ASSERT(effector, format("Actor::getEffector: actor doesn't have effector %s", Effector::convertToString(effectorIDs[i])));
Actions children;
effector->getDirectChildren(action, children);
GB_ASSERT(children.empty(), "Actor::releaseActionEffectors: action isn't allowed to have children on effector at this moment");
Actions becameActive;
effector->removeAction(action,becameActive);
for(size_t j=0; j<becameActive.size(); j++)
{
m_activatedEffectors[becameActive[j]].push_back(effectorIDs[i]);
}
}
}
/* normal construction test */
void EffectorTest::normalConstruction() {
int id = 1;
std::string name = "test";
float mass = 2;
float length = 3;
using namespace RoboticArm;
Effector createdPart = Effector(id, name, mass, length);
if (createdPart.getId() == id && createdPart.getName() == name &&
createdPart.getLength() == length && createdPart.getMass() == mass) {
CPPUNIT_ASSERT(true);
} else {
CPPUNIT_ASSERT(false);
}
}
void EffectorTest::validParReadBack(){
bool success = true;
int id = 111;
std::string name = "test";
float mass = 123.54566;
float length = 14676.54;
using namespace RoboticArm;
try {
Effector createdPart = Effector(id, name, mass, length);
if (id != createdPart.getId()) {
std::cout << "ID is wrong." << std::endl;
success = false;
}
if (name != createdPart.getName()) {
std::cout << "Name is wrong." << std::endl;
success = false;
}
if (mass != createdPart.getMass()) {
std::cout << "Mass is wrong." << std::endl;
success = false;
}
if (length != createdPart.getLength()) {
std::cout << "Length is wrong." << std::endl;
success = false;
}
} catch (const std::invalid_argument& ia) {
success = false;
}
CPPUNIT_ASSERT(success);
}
void Actor::releaseAction(Action* action)
{
//GB_INFO("ReleaseAction begin: %s (%p)",action->getDescString().c_str(), action);
static std::string tabs;
//GB_INFO("%sReleaseAction: %s (%p)",tabs.c_str(), action->getDescString().c_str(), action);
tabs.append(" ");
GB_ASSERT(m_actionsToDelete.find(action) == m_actionsToDelete.end(),
"Action has already been deleted. Action logic is corrupted!");
m_actionsToDelete.insert(action);
//terminate childs
Actions childs;
action->getChilds(childs);
for (Actions::iterator it=childs.begin(), end=childs.end();
it!=end; it++)
{
Action* child = *it;
child->terminate(Action::CANCELED);
}
//remove from effectors
const EffectorIDVector& effectorIDs = action->getRequiredEffectors();
for (size_t i=0;i<effectorIDs.size();i++)
{
Effector* effector = getEffector(effectorIDs[i]);
GB_ASSERT(effector, format("Actor::getEffector: actor doesn't have effector %s", Effector::convertToString(effectorIDs[i])));
Actions becameActive;
effector->removeAction(action,becameActive);
for(size_t j=0; j<becameActive.size(); j++)
{
m_activatedEffectors[becameActive[j]].push_back(effectorIDs[i]);
}
}
tabs.erase(tabs.size()-4,4);
//GB_INFO("ReleaseAction end: %s",action->getDescString().c_str());
}
void BehaviorNode::Attach(BehaviorNode* pAttachment, bool bIsPrecondition, bool bIsEffector, bool bIsTransition)
{
BEHAVIAC_UNUSED_VAR(bIsTransition);
BEHAVIAC_ASSERT(bIsTransition == false);
if (bIsPrecondition)
{
BEHAVIAC_ASSERT(!bIsEffector);
Precondition* predicate = (Precondition*)pAttachment;
BEHAVIAC_ASSERT(predicate != NULL);
this->m_preconditions.push_back(predicate);
Precondition::EPhase phase = predicate->GetPhase();
if (phase == Precondition::E_ENTER)
{
this->m_enter_precond++;
}
else if (phase == Precondition::E_UPDATE)
{
this->m_update_precond++;
}
else if (phase == Precondition::E_BOTH)
{
this->m_both_precond++;
}
else
{
BEHAVIAC_ASSERT(false);
}
}
else if (bIsEffector)
{
BEHAVIAC_ASSERT(!bIsPrecondition);
Effector* effector = (Effector*)pAttachment;
BEHAVIAC_ASSERT(effector != NULL);
this->m_effectors.push_back(effector);
Effector::EPhase phase = effector->GetPhase();
if (phase == Effector::E_SUCCESS)
{
this->m_success_effectors++;
}
else if (phase == Effector::E_FAILURE)
{
this->m_failure_effectors++;
}
else if (phase == Effector::E_BOTH)
{
this->m_both_effectors++;
}
else
{
BEHAVIAC_ASSERT(false);
}
}
else
{
this->m_events.push_back(pAttachment);
}
}