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;
}
int ORO_main(int argc, char *argv[])
{
po::options_description desc("Options");
desc.add_options()
("help", "show all available options supported by this deployment")
("prefix", po::value<std::string>(), "Sets a prefix for all TaskContext names")
#ifdef OROGEN_SERVICE_DISCOVERY_ACTIVATED
("sd-domain", po::value<std::string>(), "set service discovery domain")
#endif // OROGEN_SERVICE_DISOCVERY_ACTIVATED
("with-ros", po::value<bool>()->default_value(false), "also publish the task as ROS node, default is false")
("rename", po::value< std::vector<std::string> >(), "rename a task of the deployment: --rename oldname:newname");
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
if (vm.count("help")) {
std::cout << desc << std::endl;
return 0;
}
RTT::types::TypekitRepository::Import(new RTT::types::RealTimeTypekitPlugin);
RTT::types::TypekitRepository::Import(new RTT::corba::CorbaLibPlugin);
RTT::types::TypekitRepository::Import(new RTT::mqueue::MQLibPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::clientTypekitPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::clientCorbaTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::clientMQueueTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::clientTypelibTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::baseTypekitPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::baseCorbaTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::baseMQueueTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::baseTypelibTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::loggerTypekitPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::loggerCorbaTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::loggerMQueueTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::loggerTypelibTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::stdTypekitPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::stdCorbaTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::stdMQueueTransportPlugin);
RTT::types::TypekitRepository::Import(new orogen_typekits::stdTypelibTransportPlugin);
///// ApplicationServer ///
std::string prefix = "";
if (vm.count("prefix"))
prefix = vm["prefix"].as<std::string>();
bool with_ros = false;
if (vm.count("with-ros"))
with_ros = vm["with-ros"].as<bool>();
std::string task_name;
std::map<std::string, std::string> rename_map;
if (vm.count("rename")) {
const std::vector< std::string> &ren_vec = vm["rename"].as<std::vector <std::string> >();
for (unsigned int i = 0; i < ren_vec.size(); i++) {
const std::string &ren_str = ren_vec.at(i);
unsigned int colon_pos = ren_str.find(':');
if (colon_pos == std::string::npos) continue;
rename_map.insert(std::pair<std::string, std::string>(
ren_str.substr(0, colon_pos), ren_str.substr(colon_pos + 1)));
}
}
// Initialize some global threads so that we can properly setup their threading
// parameters
RTT::internal::GlobalEngine::Instance(ORO_SCHED_OTHER, RTT::os::LowestPriority);
//First Create all Tasks to be able to set some (slave-) activities later on in the second loop
task_name = "orogen_default_client__TaskB";
if (rename_map.count(task_name))
task_name = rename_map[task_name];
else
task_name = prefix + task_name;
// Implem Begin
std::auto_ptr<RTT::TaskContext> task_orogen_default_client__TaskB(
orogen::create_client__TaskB(task_name));
///////////////////////
////////// Instance of TaskContextServerFactory create a taskcontext server object
////////// based upon the desired implementation option.
bool testing = false;
bool loadPluginSucces = true;
RTT::Communication::ITaskContextServer::shared_ptr tcs;
boost::shared_ptr<RTT::plugin::PluginLoader> ploader = RTT::plugin::PluginLoader::Instance();
void *handle;
handle = ploader->getHandler("rtt_corba_remote");
typedef RTT::Communication::ITaskContextServer::shared_ptr(*createTaskServer_sig)(RTT::TaskContext *, int, char **);
if (handle != 0) {
createTaskServer_sig createTaskServer = (createTaskServer_sig) dlsym(handle, "CreateService");
const char *dlsym_error = dlerror();
if (dlsym_error) {
std::cerr << "this plugin is not a TaskContextServer\n";
loadPluginSucces = false;
} else {
std::cout << "Success: execute createTaskServer()\n";
tcs = createTaskServer(task_orogen_default_client__TaskB.get(), argc, argv);
}
} else {
loadPluginSucces = false;
}
if (!loadPluginSucces && !testing) {
RTT::corba::ApplicationServer::InitOrb(argc, argv);
RTT::Communication::TaskContextServerCollection taskContextServers;
RTT::corba::TaskContextServer::Create(task_orogen_default_client__TaskB.get());
RTT::corba::CorbaDispatcher::Instance(task_orogen_default_client__TaskB->ports(), ORO_SCHED_OTHER, RTT::os::LowestPriority);
}
///// implem END
task_name = "orogen_default_client__TaskB_Logger";
if (rename_map.count(task_name))
task_name = rename_map[task_name];
else
task_name = prefix + task_name;
std::auto_ptr<RTT::TaskContext> task_orogen_default_client__TaskB_Logger(
orogen::create_logger__Logger(task_name));
RTT::Communication::ITaskContextServer::shared_ptr loggerTCS =
RTT::Communication::TaskContextServerFactory::GetInstance()->CreateTaskContextServer("CORBA_TCS", task_orogen_default_client__TaskB_Logger.get(), argc, argv);
// Loggers
/*
RTT::corba::TaskContextServer::Create( task_orogen_default_client__TaskB_Logger.get() );
RTT::corba::CorbaDispatcher::Instance( task_orogen_default_client__TaskB_Logger->ports(), ORO_SCHED_OTHER, RTT::os::LowestPriority );
*/
//Create all Activities afterwards to be sure all tasks are created. The Activitied are also handeld by the deployment because
//the order needs to be known since slav activities are useable
//
//B WICHTIG !!!
RTT::Activity *activity_orogen_default_client__TaskB = new RTT::Activity(
ORO_SCHED_OTHER,
RTT::os::LowestPriority,
0,
task_orogen_default_client__TaskB->engine(),
"orogen_default_client__TaskB");
{
RTT::os::Thread *thread = dynamic_cast<RTT::os::Thread *>(activity_orogen_default_client__TaskB);
if (thread)
thread->setStopTimeout(10);
}
task_orogen_default_client__TaskB->setActivity(activity_orogen_default_client__TaskB);
//B Wichtig
RTT::Activity *activity_orogen_default_client__TaskB_Logger = new RTT::Activity(
ORO_SCHED_OTHER,
RTT::os::LowestPriority,
0,
task_orogen_default_client__TaskB_Logger->engine(),
"orogen_default_client__TaskB_Logger");
{
RTT::os::Thread *thread = dynamic_cast<RTT::os::Thread *>(activity_orogen_default_client__TaskB_Logger);
if (thread)
thread->setStopTimeout(10);
}
task_orogen_default_client__TaskB_Logger->setActivity(activity_orogen_default_client__TaskB_Logger);
Deinitializer deinit;
#ifdef OROGEN_SERVICE_DISCOVERY_ACTIVATED
if (vm.count("sd-domain")) {
servicediscovery::avahi::ServiceConfiguration sd_conf_orogen_default_client__TaskB(task_orogen_default_client__TaskB->getName(), vm["sd-domain"].as<std::string>());
sd_conf_orogen_default_client__TaskB.setDescription("IOR", RTT::corba::TaskContextServer::getIOR(task_orogen_default_client__TaskB.get()));
sd_conf_orogen_default_client__TaskB.setDescription("TASK_MODEL", "client::TaskB");
servicediscovery::avahi::ServiceDiscovery *sd_orogen_default_client__TaskB = new servicediscovery::avahi::ServiceDiscovery();
deinit << *sd_orogen_default_client__TaskB;
sd_orogen_default_client__TaskB->start(sd_conf_orogen_default_client__TaskB);
servicediscovery::avahi::ServiceConfiguration sd_conf_orogen_default_client__TaskB_Logger(task_orogen_default_client__TaskB_Logger->getName(), vm["sd-domain"].as<std::string>());
sd_conf_orogen_default_client__TaskB_Logger.setDescription("IOR", RTT::corba::TaskContextServer::getIOR(task_orogen_default_client__TaskB_Logger.get()));
sd_conf_orogen_default_client__TaskB_Logger.setDescription("TASK_MODEL", "logger::Logger");
servicediscovery::avahi::ServiceDiscovery *sd_orogen_default_client__TaskB_Logger = new servicediscovery::avahi::ServiceDiscovery();
deinit << *sd_orogen_default_client__TaskB_Logger;
sd_orogen_default_client__TaskB_Logger->start(sd_conf_orogen_default_client__TaskB_Logger);
}
#endif // OROGEN_SERVICE_DISCOVERY_ACTIVATED
// Start some activities
deinit << *activity_orogen_default_client__TaskB;
deinit << *activity_orogen_default_client__TaskB_Logger;
if (with_ros) {
throw std::runtime_error("Requesting to start as ROS node, but the support for 'ros' transport is not available. Recompile with 'ros' transport option!");
}
/** Setup shutdown procedure on SIGINT. We use a pipe-based channel to do
so, as we can't shutdown the ORB from the signal handler */
if (pipe(sigint_com) == -1) {
std::cerr << "failed to setup SIGINT handler: " << strerror(errno) << std::endl;
return 1;
}
struct sigaction sigint_handler;
sigint_handler.sa_handler = &sigint_quit_orb;
sigemptyset(&sigint_handler.sa_mask);
sigint_handler.sa_flags = 0;
sigint_handler.sa_restorer = 0;
if (-1 == sigaction(SIGINT, &sigint_handler, 0)) {
std::cerr << "failed to install SIGINT handler" << std::endl;
return 1;
}
sigset_t unblock_sigint;
sigemptyset(&unblock_sigint);
sigaddset(&unblock_sigint, SIGINT);
if (-1 == sigprocmask(SIG_UNBLOCK, &unblock_sigint, NULL)) {
std::cerr << "failed to install SIGINT handler" << std::endl;
return 1;
}
if (loadPluginSucces) {
tcs->Start();
} else if (!loadPluginSucces && !testing) {
RTT::corba::TaskContextServer::ThreadOrb(ORO_SCHED_OTHER, RTT::os::LowestPriority, 0);
}
while (true) {
uint8_t dummy;
int read_count = read(sigint_com[0], &dummy, 1);
if (read_count == 1)
break;
}
if (loadPluginSucces) {
tcs->Stop();
} else if (!loadPluginSucces && !testing) {
RTT::corba::TaskContextServer::ShutdownOrb();
RTT::corba::TaskContextServer::DestroyOrb();
}
return 0;
}
