FSM * buildFSM( FSMDescrip & fsmDescrip, Agents::SimulatorInterface * sim, bool VERBOSE ) {
// Acquire the spatial query instance
SPATIAL_QUERY = sim->getSpatialQuery();
//Global Simulator interface
SIMULATOR = sim;
bool valid = true;
const size_t AGT_COUNT = sim->getNumAgents();
FSM * fsm = new FSM( sim );
// Build the fsm
//we'll need this iterator later
std::vector< VelModifier * >::iterator vItr;
// Map of state names to state IDs.
std::map< std::string, size_t > stateNameMap;
// Copy the GoalSets
fsm->_goalSets.clear();
fsm->_goalSets.insert( fsmDescrip._goalSets.begin(), fsmDescrip._goalSets.end() );
fsmDescrip._goalSets.clear();
// 1. Create states
// a. Add velocity components and actions
// b. add to fsm
std::list< StateDescrip * >::const_iterator sItr = fsmDescrip._states.begin();
for ( ; sItr != fsmDescrip._states.end(); ++sItr ) {
StateDescrip * sData = *sItr;
State * s = fsmDescrip.addState( sData );
if ( s == 0x0 ) {
logger << Logger::ERR_MSG << "Error creating state!";
delete fsm;
return 0x0;
}
if ( VERBOSE ) logger << Logger::INFO_MSG << "\tAdding state: " << s->getName() << "(" << s->getID() << ")\n";
// State's goal selector
GoalSelector * gs = sData->_goalSelector;
if ( gs == 0x0 ) {
logger << Logger::WARN_MSG << "The state " << sData->_name << " doesn't specify a goal selector. The identity goal selector will be used.";
gs = new IdentityGoalSelector();
}
try {
gs->setGoalSet( fsm->getGoalSets() ); // possibly throws GoalSelectorException
s->setGoalSelector( gs ); // possibly throws GoalSelectorException
} catch ( GoalSelectorException ) {
logger << Logger::ERR_MSG << "Problem initializing the goal selector for the state " << s->getName() << ".";
delete fsm;
return 0x0;
}
sData->_goalSelector = 0x0;
// construct each velocity component
if ( sData->_velComponent == 0x0 ) {
logger << Logger::WARN_MSG << "The state " << sData->_name << " doesn't specify a velocity component. The zero velocity component will be used.";
s->setVelComponent( new ZeroVelComponent() );
} else {
s->setVelComponent( sData->_velComponent );
sData->_velComponent = 0x0;
}
// transfer each action
std::list< Action * >::iterator aItr = sData->_actions.begin();
for ( ; aItr != sData->_actions.end(); ++aItr ) {
s->addAction( *aItr );
}
sData->_actions.clear();
//transfer velocity modifiers from the state description
vItr = sData->_velModifiers.begin();
for ( ; vItr != sData->_velModifiers.end(); ++vItr ) {
s->addVelModifier( *vItr );
}
sData->_velModifiers.clear();
// Set start node
size_t stateID = fsm->addNode( s );
stateNameMap[ sData->_name ] = stateID;
}
// Connect all shared goal selectors
std::map< std::string, size_t >::iterator stateItr = stateNameMap.begin();
for ( ; stateItr != stateNameMap.end(); ++stateItr ) {
std::string stateName = stateItr->first;
size_t stateID = stateItr->second;
State * state = fsm->getNode( stateID );
SharedGoalSelector * gs = dynamic_cast< SharedGoalSelector * >( state->getGoalSelector() );
if ( gs != 0x0 ) {
if ( stateNameMap.count( gs->_stateName ) == 0 ) {
logger << Logger::ERR_MSG << "Found shared goal selector defined on line " << gs->_lineNo << ", but unable to locate state with the provided name: \"" << gs->_stateName << "\".";
delete fsm;
return 0x0;
}
State * src = fsm->getNode( stateNameMap[ gs->_stateName ] );
if ( dynamic_cast< SharedGoalSelector * >( src->getGoalSelector() ) ) {
logger << Logger::ERR_MSG << "Shared goal selector defined on line " << gs->_lineNo << " references a state with a shared goal. The source state must have a full goal selector definition.";
delete fsm;
return 0x0;
}
state->clearGoalSelector();
GoalSelector * srcGS = src->getGoalSelector();
srcGS->setPersistence( true );
state->setGoalSelector( srcGS );
}
}
if ( VERBOSE ) logger << Logger::INFO_MSG << "There are " << fsmDescrip._transitions.size() << " transitions\n";
// 2. Create transitions
std::map< std::string, std::list< Transition * > >::iterator stItr = fsmDescrip._transitions.begin();
for ( ; stItr != fsmDescrip._transitions.end(); ++stItr ) {
const std::string fromName = stItr->first;
std::list< Transition * > & tList = stItr->second;
// Determine if the origin state is valid
if ( fsmDescrip._stateNameMap.find( fromName ) == fsmDescrip._stateNameMap.end() ) {
logger << Logger::ERR_MSG << "Transition with invalid from node name: " << fromName << ".";
delete fsm;
return 0x0;
}
// Try to connect the transitions to the destination(s)
std::list< Transition * >::iterator tItr = tList.begin();
for ( ; tItr != tList.end(); ++tItr ) {
Transition * t = *tItr;
if ( ! t->connectStates( fsmDescrip._stateNameMap ) ) {
delete fsm;
return 0x0;
}
fsm->addTransition( stateNameMap[ fromName ], t );
}
tList.clear();
}
fsmDescrip._transitions.clear();
//////////////
// copy over the velocity modifiers
vItr = fsmDescrip._velModifiers.begin();
for ( ; vItr != fsmDescrip._velModifiers.end(); ++vItr ) { //TODO: replace global vel mod initalizer
fsm->addVelModifier( *vItr );
}
fsmDescrip._velModifiers.clear();
// 3. Query simulator and fsm for possible reasons to have a task
fsm->collectTasks();
for ( std::list< Task * >::iterator itr = fsmDescrip._tasks.begin();
itr != fsmDescrip._tasks.end();
++itr ) {
fsm->addTask( (*itr) );
}
fsmDescrip._tasks.clear();
// spatial query and elevation tasks
fsm->addTask( SPATIAL_QUERY->getTask() );
if ( sim->getElevationInstance() ) {
// NOTE: The elevation instance is null because none were specified.
// Eventually, the default elevation will be set.
// HOWEVER, if the default ever changes such that it requires a task,
// this won't catch it!!! So, make sure the default never requires
// a task.
fsm->addTask( sim->getElevationInstance()->getTask() );
}
logger << Logger::INFO_MSG << "There are " << fsm->getTaskCount() << " registered tasks.\n";
fsm->doTasks();
// 5. Initialize all agents
if ( VERBOSE ) logger << Logger::INFO_MSG << "Initializing agents:\n";
Agents::SimulatorState * initState = sim->getInitialState();
for ( size_t a = 0; a < AGT_COUNT; ++a ) {
Agents::BaseAgent * agt = sim->getAgent( a );
// update current state to class-appropriate value
const std::string stateName = initState->getAgentState( agt->_id );
std::map< std::string, size_t >::iterator stateIDItr = stateNameMap.find( stateName );
if ( stateIDItr == stateNameMap.end() ) {
logger << Logger::ERR_MSG << "Agent " << agt->_id << " requested to start in an unknown state: " << stateName << ".";
delete fsm;
return 0x0;
}
size_t stateID = stateIDItr->second;
// initialize velocity to preferred velocity
State * cState = fsm->getNode( stateID );
if ( VERBOSE ) {
logger << Logger::INFO_MSG << "Agent " << agt->_id << " starts in " << cState->getName() << ".";
}
fsm->setCurrentState( agt, stateID );
cState->enter( agt );
// TODO: Restore support for defining inital velocity state: zero or preferred
agt->_vel.set( Vector2( 0.f, 0.f ) );
//register the agent for all vel modifiers
vItr = fsm->_velModifiers.begin();
for ( ; vItr != fsm->_velModifiers.end(); ++vItr ) { //TODO: replace global vel mod initalizer
( *vItr )->registerAgent(agt);
}
}
ACTIVE_FSM = fsm;
return fsm;
}
