Ejemplo n.º 1
0
TEST_F(KDLCORBAPluginTest, VectorTest) {

   KDL::Vector v1(1.,2.,3.);
   KDL::Vector v2(3.,2.,1.);
   KDL::Vector v3(4.,5.,6.);

   //Set remote property
   RTT::Property<KDL::Vector>* prop = tc->properties()->getPropertyType<KDL::Vector>("vectorProperty");
   ASSERT_FALSE (prop == 0);
   prop->set(v1);
   EXPECT_EQ(prop->get(),v1) << "Failed to set remote KDL::Vector property";

   // Call vector operation (return current value of prop as return value and sets argument as new value)
   RTT::OperationCaller<KDL::Vector (const KDL::Vector& vector_in)> op = tc->getOperation("vectorOperation");
   KDL::Vector v4 = op(v2);
   EXPECT_EQ(v4,v1) << "Failed to call remote operation with KDL type";

   //Write new value to port, will set current value of prop to outport and prop to new value
   RTT::OutputPort<KDL::Vector> wport;
   RTT::InputPort<KDL::Vector> rport;
   depl.addPort(wport);
   depl.addPort(rport);
   ASSERT_TRUE(wport.connectTo(tc->getPort("VectorIn")) && wport.connected()) << "Failed to connect to remote input port";
   ASSERT_TRUE(rport.connectTo(tc->getPort("VectorOut")) && rport.connected()) << "Failed to connect to remote output port";

   wport.write(v3);
   //wait for remote to handle write
   sleep(1);
   KDL::Vector v5;
   ASSERT_EQ(rport.read(v5),RTT::NewData) << "Failed to read data from port";

   //Check if read value equals last value of property:
   EXPECT_EQ(v5,v2) << "Failed to read KDL Vector from port";
   //Check if current value of property is the written value:
   KDL::Vector v6 = prop->get();
   EXPECT_EQ(v6,v3) << "Failed to write KDL Vector to port";
}
Ejemplo n.º 2
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;

}
Ejemplo n.º 3
0
 void write(const T &sample)
 {
     localPort->write(sample);
 };