Go to the documentation of this file.00001 #include <stdlib.h>
00002
00003 #include <gtest/gtest.h>
00004 #include <ocl/CorbaDeploymentComponent.hpp>
00005 #include <rtt/os/main.h>
00006 #include <rtt/TaskContext.hpp>
00007 #include <rtt/transports/corba/TaskContextServer.hpp>
00008
00009 #include "kdlTypekitTypes.hpp"
00010
00011 class KDLCORBAPluginTest : public testing::Test {
00012
00013 protected:
00014 OCL::CorbaDeploymentComponent depl;
00015 RTT::TaskContext* tc;
00016
00017 KDLCORBAPluginTest():
00018 depl("Deployer"), tc(0)
00019 {}
00020
00021 virtual void SetUp() {
00022
00023 system("./setupcomponent.ops -d");
00024
00025 sleep(3);
00026
00027 ASSERT_TRUE(RTT::corba::TaskContextServer::InitOrb( 0,0 )) << "Failed to bring up CORBA";
00028 RTT::corba::TaskContextServer::ThreadOrb();
00029 ASSERT_TRUE(depl.import("kdl_typekit"));
00030 ASSERT_TRUE(depl.loadComponent("testcomponent","CORBA"));
00031
00032 tc = 0;
00033 tc = depl.getPeer("testcomponent");
00034 ASSERT_FALSE(tc == 0);
00035 }
00036 virtual void TearDown(){
00037 depl.shutdownDeployment();
00038 RTT::corba::TaskContextServer::ShutdownOrb();
00039 RTT::corba::TaskContextServer::DestroyOrb();
00040 system("pkill -f setupcomponent.ops");
00041 }
00042 };
00043
00044 TEST_F(KDLCORBAPluginTest, VectorTest) {
00045
00046 KDL::Vector v1(1.,2.,3.);
00047 KDL::Vector v2(3.,2.,1.);
00048 KDL::Vector v3(4.,5.,6.);
00049
00050
00051 RTT::Property<KDL::Vector>* prop = tc->properties()->getPropertyType<KDL::Vector>("vectorProperty");
00052 ASSERT_FALSE (prop == 0);
00053 prop->set(v1);
00054 EXPECT_EQ(prop->get(),v1) << "Failed to set remote KDL::Vector property";
00055
00056
00057 RTT::OperationCaller<KDL::Vector (const KDL::Vector& vector_in)> op = tc->getOperation("vectorOperation");
00058 KDL::Vector v4 = op(v2);
00059 EXPECT_EQ(v4,v1) << "Failed to call remote operation with KDL type";
00060
00061
00062 RTT::OutputPort<KDL::Vector> wport;
00063 RTT::InputPort<KDL::Vector> rport;
00064 depl.addPort(wport);
00065 depl.addPort(rport);
00066 ASSERT_TRUE(wport.connectTo(tc->getPort("VectorIn")) && wport.connected()) << "Failed to connect to remote input port";
00067 ASSERT_TRUE(rport.connectTo(tc->getPort("VectorOut")) && rport.connected()) << "Failed to connect to remote output port";
00068
00069 wport.write(v3);
00070
00071 sleep(1);
00072 KDL::Vector v5;
00073 ASSERT_EQ(rport.read(v5),RTT::NewData) << "Failed to read data from port";
00074
00075
00076 EXPECT_EQ(v5,v2) << "Failed to read KDL Vector from port";
00077
00078 KDL::Vector v6 = prop->get();
00079 EXPECT_EQ(v6,v3) << "Failed to write KDL Vector to port";
00080 }
00081
00082 int ORO_main(int argc, char **argv){
00083 testing::InitGoogleTest(&argc,argv);
00084 return RUN_ALL_TESTS();
00085 }
00086