int main(int argc, char** argv)
{
ros::init(argc, argv, "mtconnect_state_machine");
StateMachine s;
if (s.init())
{
s.run();
}
return 0;
}
int main()
{
// Makes sure ResourceManager initializes before everything else.
ResourceManager& resourceManager = ResourceManager::getInstance();
resourceManager.init();
// Run the stateMachine
StateMachine stateMachine;
stateMachine.run();
// Wait for the console window to be closed.
std::cout << "\n\n";
LOGC("You can close the program when you have read the log above :)\nBe patient for this window to close");
while (true) {
Sleep(100);
}
return EXIT_SUCCESS;
}
GEP_UNITTEST_TEST(StateMachine, INSERT_TEST_NAME_HERE)
{
GEP_UNITTEST_SETUP_EVENT_GLOBALS;
auto& logging =
TestLogging::instance();
//DummyLogging::instance();
// set up update framework
_updateFramework.setElapsedTime(10.0f);
// Add at least one callback so we know that we are still updating
size_t frameCount = 0;
_updateFramework.registerUpdateCallback([&](float){ logging.logWarning("frame %u:", frameCount); });
StateMachineFactory factory(&g_stdAllocator);
StateMachine* pMainFsm = nullptr;
// Test data.
/// TODO Insert test data here ///////////////////////////////////////////
// Actual tests. Don't put test data in here that has to be referenced
// by the state machines, as they will get out of scope when pMainFsm
// is actually run.
{
pMainFsm = factory.create("fsm");
/// TODO Insert test code here ///////////////////////////////////////////
}
pMainFsm->setLogging(&logging);
// Run the machine
pMainFsm->run(_updateFramework);
for(frameCount = 0; frameCount < 20; ++frameCount)
{
_updateFramework.run();
}
}
GEP_UNITTEST_TEST(StateMachine, UpdateStepBehavior)
{
GEP_UNITTEST_SETUP_EVENT_GLOBALS;
auto& logging =
TestLogging::instance();
//DummyLogging::instance();
// set up update framework
_updateFramework.setElapsedTime(10.0f);
// Add at least one callback so we know that we are still updating
size_t frameCount = 0;
_updateFramework.registerUpdateCallback([&](float){ logging.logWarning("frame %u:", frameCount); });
StateMachineFactory factory(&g_stdAllocator);
StateMachine* pMainFsm = nullptr;
// Test data.
size_t conditionCheckCount = 0;
size_t updateCount = 0;
// Actual tests. Don't put test data in here that has to be referenced
// by the state machines, as they will get out of scope when pMainFsm
// is actually run.
{
pMainFsm = factory.create("fsm");
pMainFsm->setLogging(&logging);
auto pState_A = pMainFsm->create<State>("A");
auto pState_B = pMainFsm->create<State>("B");
auto pState_C = pMainFsm->create<State>("C");
pState_C->setLeaveCondition([](){ return true; });
// Adding state transitions
pMainFsm->addTransition("__enter", "A");
pMainFsm->addTransition("A", "B", [&](){
// This should be hit
conditionCheckCount++;
return true;
});
pMainFsm->addTransition("B", "C", [&](){
// This should not be hit
conditionCheckCount++;
return true;
});
pMainFsm->addTransition("C", "__leave");
pState_B->getLeaveEvent()->registerListener([&](State::LeaveEventData* pData){
GEP_ASSERT(conditionCheckCount == 1, "Condition check 'A->B' not performed!");
conditionCheckCount = 0;
pData->setNextState(pState_C);
return gep::EventResult::Handled;
});
pState_C->getLeaveEvent()->registerListener([&](State::LeaveEventData*){
GEP_ASSERT(conditionCheckCount == 0, "Condition check 'B->C' should not have been performed!");
conditionCheckCount = 0;
return gep::EventResult::Handled;
});
// Set update event listeners. They should use the UpdateStepBehavior options.
//////////////////////////////////////////////////////////////////////////
pState_A->getUpdateEvent()->registerListener([&](State::UpdateEventData* pData){
pData->setUpdateStepBehavior(State::UpdateStepBehavior::Leave);
updateCount++;
return gep::EventResult::Handled;
});
pState_B->getUpdateEvent()->registerListener([&](State::UpdateEventData* pData){
pData->setUpdateStepBehavior(State::UpdateStepBehavior::LeaveWithNoConditionChecks);
return gep::EventResult::Handled;
});
}
pMainFsm->setLogging(&logging);
// Run the machine
pMainFsm->run(_updateFramework);
for(frameCount = 0; frameCount < 5; ++frameCount)
{
_updateFramework.run();
}
GEP_ASSERT(updateCount > 0, "State A should have at least be updated once!");
}
GEP_UNITTEST_TEST(StateMachine, Basics)
{
GEP_UNITTEST_SETUP_EVENT_GLOBALS;
auto& logging =
TestLogging::instance();
//DummyLogging::instance();
// set up update framework
_updateFramework.setElapsedTime(10.0f);
// Add at least one callback so we know that we are still updating
size_t frameCount = 0;
_updateFramework.registerUpdateCallback([&](float){ logging.logWarning("frame %u:", frameCount); });
StateMachineFactory factory(&g_stdAllocator);
StateMachine* pMainFsm = nullptr;
// test data
size_t testData_A = 0;
size_t testData_B = 0;
const size_t numTestStages = 9;
size_t testStages[numTestStages] = { 0 };
// actual test
{
auto pFsm = pMainFsm = factory.create("fsm");
pFsm->setLogging(&logging);
// Adding states
auto pState_A = pFsm->create<State>("A");
auto state_B = pFsm->create<State>("B");
// Adding state transitions
pFsm->addTransition("__enter", "A");
pFsm->addTransition("A", "B");
pFsm->addTransition("B", "__leave");
// Set state leave conditions
pState_A->setLeaveCondition([&](){
return frameCount == 3;
});
state_B->setLeaveCondition([&](){
return frameCount == 6;
});
// Set some listeners
pFsm->getEnterEvent()->registerListener([&](State::EnterEventData*){
testStages[0]++;
return gep::EventResult::Handled;
});
pFsm->getLeaveEvent()->registerListener([&](State::LeaveEventData*){
testStages[1]++;
return gep::EventResult::Handled;
});
pFsm->getUpdateEvent()->registerListener([&](State::UpdateEventData*){
testStages[2]++;
return gep::EventResult::Handled;
});
pState_A->getEnterEvent()->registerListener([&](State::EnterEventData*){
testStages[3]++;
return gep::EventResult::Handled;
});
pState_A->getLeaveEvent()->registerListener([&](State::LeaveEventData*){
testStages[4]++;
return gep::EventResult::Handled;
});
pState_A->getUpdateEvent()->registerListener([&](State::UpdateEventData*){
testStages[5]++;
testData_A++;
return gep::EventResult::Handled;
});
state_B->getEnterEvent()->registerListener([&](State::EnterEventData*){
testStages[6]++;
return gep::EventResult::Handled;
});
state_B->getLeaveEvent()->registerListener([&](State::LeaveEventData*){
testStages[7]++;
return gep::EventResult::Handled;
});
state_B->getUpdateEvent()->registerListener([&](State::UpdateEventData*){
testStages[8]++;
testData_B++;
return gep::EventResult::Handled;
});
}
pMainFsm->setLogging(&logging);
// Run the machine
pMainFsm->run(_updateFramework);
for(frameCount = 0; frameCount < 10; ++frameCount)
{
_updateFramework.run();
}
}
bool Avoidance(){
ThreadData *t = (ThreadData *)pthread_getspecific(*tlsKey);
int retries = 0;
retry: /*The label to use to break out of the inner loop when waiting*/
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,NULL);
pthread_mutex_lock(&MachinesLock);
int i = -1;
set<StateMachine *>::iterator it, et, wk;
for(it = Machines->begin(), et = Machines->end(); it != et; it++){
volatile enum SMAction a = (*it)->run( t->bt, (unsigned long)pthread_self() );
i++;
switch(a){
case WAIT:
if(retries > AVOIDANCE_MAX_RETRIES){ /*Wait 5ms at most??*/
if(t->curActiveMachines > 0){t->curActiveMachines--;}
wk = it;
wk--;
delete (*it);
Machines->erase(it);
it = wk;
break;
}
/*if(retries == 0){
fprintf(stderr,"Thread %d hit a wait point for machine %d!\n",(int)pthread_self(),i);
}*/
retries++;
pthread_mutex_unlock(&MachinesLock);
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
usleep(AVOIDANCE_WAIT);
goto retry;
case TERM:
//fprintf(stderr,"Thread %d stopped machine %d!\n",(int)pthread_self(),i);
if(t->curActiveMachines > 0){t->curActiveMachines--;}
wk = it;
wk--;
delete (*it);
Machines->erase(it);
it = wk;
break;
case CONT:
default:
break;
}/*End switch*/
}/*Done with all machines in list.*/
set<StateMachineFactory *>::iterator fit, fet;
for(fit = t->myFactories->begin(), fet = t->myFactories->end(); fit != fet; fit++){
t->numMachineStartChecks++;
if((*fit)->isStartState(t->bt)){
StateMachine *m = (*fit)->CreateMachine();
m->run(t->bt, (unsigned long)pthread_self());
t->numMachineStarts++;
/*fprintf(stderr,"Thread %d just started a machine %d at %p!\n",(int)pthread_self(),i,bt->bt[0]);*/
Machines->insert(m);
}
}
if(Machines->size() > maxMachinesGlobal){
maxMachinesGlobal = Machines->size();
}
pthread_mutex_unlock(&MachinesLock);
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
/*End Avoidance Code*/
}
