bool setupHook() {
Configuration::setStorePath("../conf");
Configuration::addPath("../conf/common");
Configuration::addPath("../conf");
Configuration::addPath("../conf/" + getRealm());
try {
// set Application echelon?
if (hasOption("level")) {
this->setEchelon(static_cast<EchelonType>(getOption<unsigned>("level")));
}
// start in simulation mode?
if (hasOption("simulation")) {
simulation = true;
// set Application echelon
if (this->getEchelon() == Echelon::None) this->setEchelon(Echelon::Sensors);
this->getMainActivity()->setPeriod(0.0);
}
// import components from this package and dependent packages
this->import(ROS_PACKAGE_NAME);
this->addTask(loadComponent("Interface", "EthernetInterface", true, Echelon::Sensors));
this->addTask(loadComponent("Navigation", "Navigation", true, Echelon::Navigation));
this->addTask(loadComponent("GPS", "GPS", false, Echelon::Sensors));
RTT::TaskContext *fmpbox = this->addTask(loadComponent("FMPBox", "Interface::FMPBox", false, Base::Sensors));
if (fmpbox) fmpbox->setActivity(new RTT::Activity(RTT::os::HighestPriority, 0, "FMPBox"));
} catch (std::runtime_error& e) {
log(Fatal) << e.what() << endlog();
return false;
}
return true;
}
int StartCommon::runCommon(state_machine::State *initialState, const std::vector< init::Base* >& toInit)
{
std::vector< init::Base* > toInitCpy(toInit);
init::TransformerBroadcaster *broadcaster = new init::TransformerBroadcaster("transformer_broadcaster", *robot);
toInitCpy.push_back(broadcaster);
init::Container all(toInitCpy);
//various init transitions
Init initializer(*transformerHelper, *configHelper, all, initialState);
state_machine::StateMachine &stateMachine(state_machine::StateMachine::getInstance());
RTT::TaskContext *clientTask = OrocosHelpers::getClientTask();
RTT::OutputPort<state_machine::serialization::Event> *eventPort = new RTT::OutputPort<state_machine::serialization::Event>();
RTT::OutputPort<state_machine::serialization::StateMachine> *dumpPort = new RTT::OutputPort<state_machine::serialization::StateMachine>();
RTT::OutputPort<std::string> *debugMessages = new RTT::OutputPort<std::string>();
clientTask->addPort("stateMachine_Events", *eventPort);
clientTask->addPort("stateMachine_Dump", *dumpPort);
clientTask->addPort("stateMachine_Msg", *debugMessages);
state_machine::serialization::StateMachine smDump(stateMachine);
if(loggingActive)
{
const std::string loggerName("taskManagement_logger");
RTT::TaskContext *logger = new logger::Logger(loggerName);
RTT::corba::TaskContextServer::Create( logger );
RTT::corba::CorbaDispatcher::Instance( logger->ports(), ORO_SCHED_OTHER, RTT::os::LowestPriority );
RTT::Activity* activity_Logger = new RTT::Activity(
ORO_SCHED_OTHER,
RTT::os::LowestPriority,
0,
logger->engine(),
"taskManagement_logger");
{ RTT::os::Thread* thread = dynamic_cast<RTT::os::Thread*>(activity_Logger);
if (thread)
thread->setStopTimeout(10);
}
logger->setActivity(activity_Logger);
orocos_cpp::LoggingHelper lHelper;
lHelper.logAllPorts(clientTask, loggerName, {}, false);
}
// if(simulationActive)
// {
//
// widget->update(smDump);
// // widget->repaint();
// app->processEvents();
// }
int cnt = 0;
stateMachine.setExecuteCallback([&](){
if(cnt >= 10)
{
cnt = 0;
dumpPort->write(smDump);
}
cnt++;
//Events for state_machine visualisation + state_machine
std::vector<state_machine::serialization::Event> newEvents = stateMachine.getNewEvents();
for(auto e: newEvents)
{
eventPort->write(e);
// if(simulationActive)
// {
// //update widget
// widget->update(e);
// // widget->repaint();
// }
}
//
// if(simulationActive)
// {
// app->processEvents();
// }
std::string debugMsgs = stateMachine.getDebugStream().str();
stateMachine.getDebugStream().str(std::string());
if(!debugMsgs.empty())
{
std::cout << debugMsgs;
debugMessages->write(debugMsgs);
}
}
);
if(loggingActive)
{
initializer.activateLogging(logExcludeList);
}
stateMachine.start(&initializer);
while (!stateMachine.execute())
{
}
return 0;
}