Пример #1
0
 ProxyWriter(RTT::base::InputPortInterface *port): localPort(dynamic_cast<RTT::OutputPort<T> * >(port->antiClone()))
 {
     if(!localPort)
         throw std::runtime_error("Error, could not create ProxyWriter");
     
     RTT::TaskContext *clientTask = getClientTask();
     clientTask->addPort(*localPort);
     localPort->connectTo(port);
 }
Пример #2
0
 ProxyReader(RTT::base::OutputPortInterface *port): localPort()
 {
     if(!localPort)
         throw std::runtime_error("Error, could not create ProxyReader");
     
     RTT::TaskContext *clientTask = getClientTask();
     clientTask->addPort(*localPort);
     port->connectTo(localPort);
 }
Пример #3
0
    float Controller::getSurface(std::string state, std::string attr){
        RTT::TaskContext* t = (TaskContext*)(getPeer(state));
        if(not t){
            return 0;
        }
        //TODO - We need to account for the case where this lookup fails
        RTT::Attribute<float> a =
            t->attributes()->getAttribute(attr);

        return a.get();
    }
Пример #4
0
 RTT::OutputPort<T> &getWriter(RTT::ConnPolicy const& policy)
 {
     if(!writer)
     {
         writer = dynamic_cast<RTT::OutputPort<T> * >(port->antiClone());
         RTT::TaskContext *clientTask = getClientTask();
         clientTask->addPort(*writer);
         port->connectTo(writer, policy);
     }
     return *writer;
 };
Пример #5
0
 RTT::InputPort<T> &getReader(RTT::ConnPolicy const& policy)
 {
     if(!reader)
     {
         reader = dynamic_cast<RTT::InputPort<T> *>(port->antiClone());
         RTT::TaskContext *clientTask = getClientTask();
         clientTask->addPort(*reader);
         reader->connectTo(port, policy);
     }
     
     return *reader;
 };    
Пример #6
0
bool RTOps::configureHook() {
    rtHandler.beginRT();

    // Connect with the connector.
    RTT::TaskContext *peer = this->getPeer("atrias_connector");
    if (!peer) {
        log(RTT::Error) << "[RTOps] Failed to connect to a Connector!" << RTT::endlog();
        return false;
    }
    sendControllerOutput = peer->provides("connector")->getOperation("sendControllerOutput");

    return true;
}
    bool connectDefault(const std::string& robot_name) // MQueue
    {
        if(hasPeer(robot_name)) {
            RTT::log(RTT::Info) << "Trying to connect to default joint ports."<<RTT::endlog();
            RTT::TaskContext* peer = getPeer(robot_name);
            if(peer == NULL) return false;
            RTT::ConnPolicy policy = RTT::ConnPolicy::data();

            port_JointPosition.connectTo(peer->getPort("JointPosition"),policy);
            port_JointVelocity.connectTo(peer->getPort("JointVelocity"),policy);
            port_JointTorque.connectTo(peer->getPort("JointTorque"),policy);
        } else {
            return false;
        }
        return port_JointPosition.connected() || port_JointVelocity.connected() || port_JointTorque.connected();
    }
OrocosCommandDispatcher::OrocosCommandDispatcher(RTT::TaskContext &task, boost::function<void (int32_t actuatorId, base::actuators::DRIVE_MODE mode, double value)> setCommandCallback, ::std::string const & name, ::std::vector< boost::int32_t > const & actuatorMap): CommandDispatcher(actuatorMap, setCommandCallback), task(task)
{
    std::string inputPortName("cmd_" + name);

	// Verify name clashes
    if (task.provides()->hasService(inputPortName))
    {
	RTT::log(RTT::Error) << "cannot create a dispatch called '" << inputPortName << "' as a port named '" << inputPortName << "' is already in use in the task interface" << RTT::endlog();
	throw std::runtime_error(std::string("cannot create a dispatch called '") + inputPortName + "' as a port named '" + inputPortName + "' is already in use in the task interface");
    }

    port = new RTT::InputPort< base::actuators::Command >(inputPortName);
    task.addPort(*port);
    
    task.provides()->addEventPort(*port);
    
    inputSample.resize(actuatorMap.size());
}
OrocsStateAggregator::OrocsStateAggregator(RTT::TaskContext& task, const std::string& name, const std::vector< int32_t >& statusMap, base::Time statusInterval) : StateAggregator(statusMap, statusInterval), task(task)
{
    std::string outputPortName("status_" + name);

    // Verify name clashes
    if (task.provides()->hasService(outputPortName))
    {
	RTT::log(RTT::Error) << "cannot create a dispatch called '" << name << "' as a port named  '" << outputPortName << "' is already in use in the task interface" << RTT::endlog();
	throw std::runtime_error(std::string("cannot create a dispatch called '") + name + "' as a port named  '" + outputPortName + "' is already in use in the task interface");
    }

    outputPort = new RTT::OutputPort< base::actuators::Status >(outputPortName);
    task.addPort(*outputPort);

    // Initialize the connections with the preallocated data structure, so that
    // further write() are hard-realtime
    outputPort->setDataSample(getStatus());
}
Пример #10
0
  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;
  }
Пример #11
0
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;

}