main.cpp
Go to the documentation of this file.
00001 #include <rtt/os/main.h>
00002 
00003 #include <boost/program_options.hpp>
00004 #include <iostream>
00005 
00006 <% if deployer.corba_enabled? %>
00007 #ifdef OROGEN_SERVICE_DISCOVERY_ACTIVATED
00008 #include <service_discovery/service_discovery.h>
00009 #endif // OROGEN_SERVICE_DISCOVERY_ACTIVATED
00010 <% end %>
00011 
00012 #include <rtt/typekit/RealTimeTypekit.hpp>
00013 <% deployer.rtt_transports.each do |transport_name| %>
00014 #include <rtt/transports/<%= transport_name %>/TransportPlugin.hpp>
00015 <% end %>
00016 
00017 <% if component.typekit || !component.used_typekits.empty? %>#include <rtt/types/TypekitPlugin.hpp><% end %>
00018 <% if typekit = component.typekit %>
00019 #include "typekit/Plugin.hpp"
00020 <% deployer.transports.each do |transport_name| %>
00021 #include "typekit/transports/<%= transport_name %>/TransportPlugin.hpp"
00022 <% end %>
00023 <% end %>
00024 <% deployer.used_typekits.each do |tk| %>
00025     <% next if tk.virtual? %>
00026 #include <<%= tk.name %>/typekit/Plugin.hpp>
00027     <% deployer.transports.each do |transport_name| %>
00028 #include <<%= tk.name %>/transports/<%= transport_name %>/TransportPlugin.hpp>
00029     <% end %>
00030 <% end %>
00031 
00032 <% task_activities = deployer.task_activities.
00033         sort_by(&:name) %>
00034 
00035 <% task_activities.each do |task| %>
00036 #include <<%= task.context.header_file %>>
00037 <% end %>
00038 <% if deployer.corba_enabled? %>
00039 #include <rtt/transports/corba/ApplicationServer.hpp>
00040 #include <rtt/transports/corba/TaskContextServer.hpp>
00041 #include <rtt/transports/corba/CorbaDispatcher.hpp>
00042 #include <signal.h>
00043 <% end %>
00044 
00045 <% require 'set'
00046     activity_headers = task_activities.
00047         map { |t| t.activity_type.header }.
00048         compact.
00049         to_set %>
00050 <% activity_headers.to_a.sort.each do |header| %>
00051 #include <<%= header %>>
00052 <% end %>
00053 
00054 <% if deployer.browse %>
00055 #include <ocl/TaskBrowser.hpp>
00056 <% end %>
00057 
00058 #include <rtt/Logger.hpp>
00059 #include <rtt/base/ActivityInterface.hpp>
00060 
00061 namespace po = boost::program_options;
00062 
00063 class Deinitializer
00064 {
00065     friend Deinitializer& operator << (Deinitializer&, RTT::base::ActivityInterface&);
00066 
00067     std::vector<RTT::base::ActivityInterface*> m_activities;
00068 
00069 <% if deployer.corba_enabled? %>
00070 #ifdef OROGEN_SERVICE_DISCOVERY_ACTIVATED
00071     friend Deinitializer& operator << (Deinitializer&, servicediscovery::ServiceDiscovery&);
00072 
00073     std::vector<servicediscovery::ServiceDiscovery*> m_service_discoveries;
00074 #endif
00075 <% end %>
00076 
00077 
00078 public:
00079     ~Deinitializer()
00080     {
00081         for (std::vector<RTT::base::ActivityInterface*>::const_iterator it = m_activities.begin();
00082                 it != m_activities.end(); ++it)
00083         {
00084             (*it)->stop();
00085         }
00086 
00087 <% if deployer.corba_enabled? %>
00088 #ifdef OROGEN_SERVICE_DISCOVERY_ACTIVATED
00089         for(std::vector<servicediscovery::ServiceDiscovery*>::iterator sit = m_service_discoveries.begin();
00090                 sit != m_service_discoveries.end(); ++sit)
00091         {
00092             (*sit)->stop();
00093             delete *sit;
00094         }
00095 #endif 
00096 <% end %>
00097     }
00098 };
00099 
00100 Deinitializer& operator << (Deinitializer& deinit, RTT::base::ActivityInterface& activity)
00101 {
00102     deinit.m_activities.push_back(&activity);
00103     return deinit;
00104 }
00105 
00106 <% if deployer.corba_enabled? %>
00107 #ifdef OROGEN_SERVICE_DISCOVERY_ACTIVATED
00108 Deinitializer& operator << (Deinitializer& deinit, servicediscovery::ServiceDiscovery& service_discovery)
00109 {
00110     deinit.m_service_discoveries.push_back(&service_discovery);
00111     return deinit;
00112 }
00113 #endif 
00114 <% end %>
00115 
00116 <% if deployer.corba_enabled? %>
00117 int sigint_com[2];
00118 void sigint_quit_orb(int)
00119 {
00120     uint8_t dummy = 0;
00121     unsigned int sent = 0;
00122     while(sent < sizeof(uint8_t))
00123     {
00124         int ret = write(sigint_com[1], &dummy, sizeof(uint8_t));
00125         if(ret < 0)
00126         {
00127             std::cerr << "Failed to signal quit to orb" << std::endl;
00128             break;
00129         }
00130         sent += ret;
00131     }
00132 }
00133 <% end %>
00134 
00135 int ORO_main(int argc, char* argv[])
00136 {
00137    po::options_description desc("Options");
00138 
00139    desc.add_options()
00140         ("help", "show all available options supported by this deployment")
00141         ("prefix", po::value<std::string>(), "Sets a prefix for all TaskContext names")
00142 <% if deployer.corba_enabled? %>
00143 #ifdef OROGEN_SERVICE_DISCOVERY_ACTIVATED
00144         ("sd-domain", po::value<std::string>(), "set service discovery domain")
00145 #endif // OROGEN_SERVICE_DISOCVERY_ACTIVATED
00146 <% end %>
00147         ("rename", po::value< std::vector<std::string> >(), "rename a task of the deployment: --rename oldname:newname");
00148 
00149    po::variables_map vm;
00150    po::store(po::parse_command_line(argc, argv, desc), vm);
00151    po::notify(vm);
00152 
00153    if(vm.count("help")) {
00154        std::cout << desc << std::endl;
00155        return 0;
00156    }
00157 
00158 
00159    <% if deployer.loglevel %>
00160    if ( log().getLogLevel() < Logger::<%= deployer.loglevel %> ) {
00161        log().setLogLevel( Logger::<%= deployer.loglevel %> );
00162    }
00163    <% end %>
00164 
00165    RTT::types::TypekitRepository::Import( new RTT::types::RealTimeTypekitPlugin );
00166    <% if deployer.transports.include?('corba') %>
00167    RTT::types::TypekitRepository::Import( new RTT::corba::CorbaLibPlugin );
00168    <% end %>
00169    <% if deployer.transports.include?('mqueue') %>
00170    RTT::types::TypekitRepository::Import( new RTT::mqueue::MQLibPlugin );
00171    <% end %>
00172 
00173    <% if component.typekit %>
00174    RTT::types::TypekitRepository::Import( new orogen_typekits::<%= component.name %>TypekitPlugin );
00175    <% deployer.transports.each do |transport_name| %>
00176    RTT::types::TypekitRepository::Import( new <%= typekit.transport_plugin_name(transport_name) %> );
00177    <% end %>
00178    <% end %>
00179    <% deployer.used_typekits.each do |tk| %>
00180    <% next if tk.virtual? %>
00181    RTT::types::TypekitRepository::Import( new orogen_typekits::<%= tk.name %>TypekitPlugin );
00182        <% deployer.transports.each do |transport_name| %>
00183    RTT::types::TypekitRepository::Import( new <%= Orocos::Generation::Typekit.transport_plugin_name(transport_name, tk.name) %> );
00184        <% end %>
00185    <% end %>
00186 
00187 <% if deployer.corba_enabled? %>
00188     RTT::corba::ApplicationServer::InitOrb(argc, argv);
00189 <% end %>
00190 
00191     std::string prefix = "";
00192 
00193     if( vm.count("prefix")) 
00194         prefix = vm["prefix"].as<std::string>();
00195 
00196     std::string task_name;
00197 
00198     std::map<std::string, std::string> rename_map;
00199 
00200     if ( vm.count("rename") ) {
00201 
00202         const std::vector< std::string>& ren_vec = vm["rename"].as<std::vector <std::string> >();
00203 
00204         for ( unsigned int i = 0; i < ren_vec.size(); i++) {
00205 
00206             const std::string& ren_str = ren_vec.at(i);
00207 
00208             unsigned int colon_pos = ren_str.find(':');
00209             if ( colon_pos == std::string::npos ) continue;
00210 
00211             rename_map.insert( std::pair<std::string, std::string>( 
00212                 ren_str.substr(0,colon_pos), ren_str.substr(colon_pos+1) ));
00213         }
00214     }    
00215 
00216 <% task_activities.each do |task| %>
00217     task_name = "<%= task.name %>";
00218     if (rename_map.count(task_name))
00219         task_name = rename_map[task_name];
00220     else
00221         task_name = prefix + task_name;
00222     
00223     <%= task.context.class_name %> task_<%= task.name%>(task_name);
00224     <%= task.generate_activity_setup %>
00225     task_<%= task.name %>.setActivity(activity_<%= task.name %>);
00226     <% task.properties.sort_by { |prop| prop.name }.each do |prop|
00227         if prop.value %>
00228     dynamic_cast< RTT::Property<  <%= prop.interface_object.type.cxx_name %> >*>(
00229             task_<%= task.name %>.properties()->getProperty("<%= prop.name %>"))
00230         ->set(<%= prop.value.inspect %>);
00231         <% end %>
00232     <% end %>
00233 
00234     <% if deployer.corba_enabled? %>
00235     RTT::corba::TaskContextServer::Create( &task_<%= task.name%> );
00236     <% if task.realtime? %>
00237     RTT::corba::CorbaDispatcher::Instance( task_<%= task.name %>.ports(), ORO_SCHED_RT, RTT::os::LowestPriority );
00238     <% else %>
00239     RTT::corba::CorbaDispatcher::Instance( task_<%= task.name %>.ports(), ORO_SCHED_OTHER, RTT::os::LowestPriority );
00240     <% end %>
00241     <% end %>
00242 
00243 <% end %>
00244 
00245    Deinitializer deinit;
00246 
00247 <% if deployer.corba_enabled? %>
00248 #ifdef OROGEN_SERVICE_DISCOVERY_ACTIVATED
00249     if( vm.count("sd-domain") ) {
00250 <% task_activities.each do |task| %>
00251     servicediscovery::ServiceConfiguration sd_conf_<%= task.name%>(prefix + "<%= task.name %>", vm["sd-domain"].as<std::string>());
00252     sd_conf_<%= task.name%>.setDescription("IOR", RTT::corba::TaskContextServer::getIOR(&task_<%= task.name%>));
00253     servicediscovery::ServiceDiscovery* sd_<%= task.name%> = new servicediscovery::ServiceDiscovery();
00254     deinit << *sd_<%= task.name%>;
00255     sd_<%= task.name%>->start(sd_conf_<%= task.name%>);
00256 <% end %>
00257     }
00258 #endif // OROGEN_SERVICE_DISCOVERY_ACTIVATED
00259 <% end %>
00260 
00261 <% if !deployer.loggers.empty?
00262         deployer.loggers.sort_by { |filename, _| filename }.each do |filename, logger|
00263             logger.config.each do |type, reported_activity, args| %>
00264                 task_<%= logger.task.name %>.connectPeers(&task_<%= reported_activity.name %>);
00265                 task_<%= logger.task.name %>.report<%= type %>(<%= args.map { |v| "\"#{v}\"" }.join(", ") %>);
00266             <% end %>
00267         <% end %>
00268 <% end %>
00269 
00270 
00271 <% deployer.peers.sort_by { |a, b| [a.name, b.name] }.each do |a, b| %>
00272     task_<%= a.name %>.connectPeers(&task_<%= b.name %>);
00273 <% end %>
00274 
00275 <% deployer.connections.
00276     sort_by { |src, dst, policy| [src.name, dst.name] }.
00277     each do |src, dst, policy|
00278         if src.kind_of?(TaskDeployment) %>
00279             task_<%= src.activity.name %>.connectPorts(&task_<%= dst.activity.name %>);
00280         <% else %>
00281         {
00282             <%= policy.to_code("policy") %>
00283             RTT::base::OutputPortInterface* src = dynamic_cast<RTT::base::OutputPortInterface*>(
00284                     task_<%= src.activity.name %>.ports()->getPort("<%= src.name %>"));
00285             RTT::base::InputPortInterface* dst = dynamic_cast<RTT::base::InputPortInterface*>(
00286                     task_<%= dst.activity.name %>.ports()->getPort("<%= dst.name %>"));
00287             src->createConnection(*dst, policy);
00288         }
00289         <% end %>
00290     <% end %>
00291 
00292     // Start some activities
00293 <% task_activities.each do |task| %>
00294     deinit << *activity_<%= task.name%>;
00295 
00296     <% if task.start?
00297         if task.context.needs_configuration? %>
00298     if (!task_<%= task.name %>.configure())
00299     {
00300         RTT::log(RTT::Error) << "cannot configure <%= task.name %>" << RTT::endlog();
00301         return -1;
00302     }
00303         <% end %>
00304     if (!task_<%= task.name %>.start())
00305     {
00306         RTT::log(RTT::Error) << "cannot start <%= task.name %>" << RTT::endlog();
00307         return -1;
00308     }
00309     <% end %>
00310 <% end %>
00311 
00312 <% if deployer.corba_enabled? %>
00315     if (pipe(sigint_com) == -1)
00316     {
00317         std::cerr << "failed to setup SIGINT handler: " << strerror(errno) << std::endl;
00318         return 1;
00319     }
00320 
00321     struct sigaction sigint_handler;
00322     sigint_handler.sa_handler = &sigint_quit_orb;
00323     sigemptyset(&sigint_handler.sa_mask);
00324     sigint_handler.sa_flags     = 0;
00325     sigint_handler.sa_restorer  = 0;
00326     if (-1 == sigaction(SIGINT, &sigint_handler, 0))
00327     {
00328         std::cerr << "failed to install SIGINT handler" << std::endl;
00329         return 1;
00330     }
00331     sigset_t unblock_sigint;
00332     sigemptyset(&unblock_sigint);
00333     sigaddset(&unblock_sigint, SIGINT);
00334     if (-1 == sigprocmask(SIG_UNBLOCK, &unblock_sigint, NULL))
00335     {
00336         std::cerr << "failed to install SIGINT handler" << std::endl;
00337         return 1;
00338     }
00339     RTT::corba::TaskContextServer::ThreadOrb();
00340     while (true)
00341     {
00342         uint8_t dummy;
00343         int read_count = read(sigint_com[0], &dummy, 1);
00344         if (read_count == 1)
00345             break;
00346     }
00347 
00348     RTT::corba::TaskContextServer::ShutdownOrb();
00349     RTT::corba::TaskContextServer::DestroyOrb();
00350 <% elsif deployer.browse %>
00351     OCL::TaskBrowser browser(& task_<%= deployer.browse.name %>);
00352     browser.loop();
00353 <% end %>
00354 
00355     return 0;
00356 }
00357 


orogen
Author(s): Sylvain Joyeux/sylvain.joyeux@m4x.org
autogenerated on Wed Sep 16 2015 10:36:40