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";
}
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;
}
void write(const T &sample)
{
localPort->write(sample);
};