コード例 #1
0
ファイル: CommonStart.cpp プロジェクト: rock-cpp/lib_init
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;

}
コード例 #2
0
ファイル: tb.cpp プロジェクト: FynnGamadeyo/Test-Setup-RTT
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;
}