$search
00001 #include <gtest/gtest.h> 00002 #include <ocl/DeploymentComponent.hpp> 00003 #include <rtt/types/TypekitRepository.hpp> 00004 #include <rtt/types/Types.hpp> 00005 #include <rtt/marsh/Marshalling.hpp> 00006 #include <kdl/frames.hpp> 00007 #include <kdl/chain.hpp> 00008 #include <kdl/jacobian.hpp> 00009 #include <kdl/jntarray.hpp> 00010 #include <rtt/os/main.h> 00011 00012 class KDLPluginTest : public testing::Test { 00013 00014 #define COMPNAME "Deployer" 00015 00016 protected: 00017 KDLPluginTest(): 00018 depl(COMPNAME), 00019 chain_prop("chain",""), 00020 frame_prop("frame",""), 00021 jacobian_prop("jacobian",""), 00022 jntarray_prop("jntarray",""), 00023 joint_prop("joint",""), 00024 rotation_prop("rotation",""), 00025 segment_prop("segment",""), 00026 twist_prop("twist",""), 00027 vector_prop("vector",""), 00028 wrench_prop("wrench","") 00029 {} 00030 00031 virtual void SetUp(){ 00032 00033 } 00034 virtual void TearDown(){ 00035 } 00036 00037 OCL::DeploymentComponent depl; 00038 KDL::Chain chain; 00039 KDL::Frame frame; 00040 KDL::Jacobian jac; 00041 KDL::JntArray jntarray; 00042 KDL::Joint joint; 00043 KDL::Rotation rotation; 00044 KDL::Segment segment; 00045 KDL::Twist twist; 00046 KDL::Vector vector; 00047 KDL::Wrench wrench; 00048 RTT::Property<KDL::Chain> chain_prop; 00049 RTT::Property<KDL::Frame> frame_prop; 00050 RTT::Property<KDL::Jacobian> jacobian_prop; 00051 RTT::Property<KDL::JntArray> jntarray_prop; 00052 RTT::Property<KDL::Joint> joint_prop; 00053 RTT::Property<KDL::Rotation> rotation_prop; 00054 RTT::Property<KDL::Segment> segment_prop; 00055 RTT::Property<KDL::Twist> twist_prop; 00056 RTT::Property<KDL::Vector> vector_prop; 00057 RTT::Property<KDL::Wrench> wrench_prop; 00058 }; 00059 00060 TEST_F(KDLPluginTest,loadTypekit) 00061 { 00062 ASSERT_TRUE(depl.import("kdl_typekit")); 00063 } 00064 00065 TEST_F(KDLPluginTest,getKDLTypes) 00066 { 00067 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::Chain>()); 00068 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::Frame>()); 00069 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::Jacobian>()); 00070 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::JntArray>()); 00071 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::Joint>()); 00072 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::Rotation>()); 00073 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::Segment>()); 00074 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::Twist>()); 00075 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::Vector>()); 00076 EXPECT_FALSE(NULL==RTT::types::Types()->getTypeInfo<KDL::Wrench>()); 00077 } 00078 //What do we want test: 00079 //Create every type as property 00080 //Write all types to file 00081 //Read all types from file 00082 //Write to other file 00083 //Read again, are they the same?? 00084 00085 00086 TEST_F(KDLPluginTest,createProperties) 00087 { 00088 EXPECT_TRUE(depl.addProperty(chain_prop)); 00089 EXPECT_TRUE(depl.getProperty(chain_prop.getName())->ready()); 00090 EXPECT_TRUE(depl.addProperty(frame_prop)); 00091 EXPECT_TRUE(depl.addProperty(jacobian_prop)); 00092 EXPECT_TRUE(depl.addProperty(jntarray_prop)); 00093 EXPECT_TRUE(depl.addProperty(joint_prop)); 00094 EXPECT_TRUE(depl.addProperty(rotation_prop)); 00095 EXPECT_TRUE(depl.addProperty(segment_prop)); 00096 EXPECT_TRUE(depl.addProperty(twist_prop)); 00097 EXPECT_TRUE(depl.addProperty(vector_prop)); 00098 EXPECT_TRUE(depl.addProperty(wrench_prop)); 00099 00100 00101 } 00102 00103 TEST_F(KDLPluginTest,writeProperties) 00104 { 00105 boost::shared_ptr<RTT::Marshalling> marsh = depl.getProvider<RTT::Marshalling>("marshalling"); 00106 //EXPECT_TRUE(depl.addProperty(chain_prop)); 00107 //EXPECT_TRUE(depl.getProperty(chain_prop.getName())->ready()); 00108 EXPECT_TRUE(depl.addProperty(frame_prop)); 00109 //EXPECT_TRUE(depl.addProperty(jacobian_prop)); 00110 //EXPECT_TRUE(depl.addProperty(jntarray_prop)); 00111 EXPECT_TRUE(depl.addProperty(joint_prop)); 00112 EXPECT_TRUE(depl.addProperty(rotation_prop)); 00113 EXPECT_TRUE(depl.addProperty(segment_prop)); 00114 EXPECT_TRUE(depl.addProperty(twist_prop)); 00115 EXPECT_TRUE(depl.addProperty(vector_prop)); 00116 EXPECT_TRUE(depl.addProperty(wrench_prop)); 00117 00118 EXPECT_TRUE(marsh->storeProperties("test/test.cpf")); 00119 EXPECT_TRUE(marsh->writeProperties("test/test1.cpf")); 00120 EXPECT_TRUE(marsh->updateFile("test/test1.cpf")); 00121 EXPECT_TRUE(marsh->readProperties("test/test.cpf")); 00122 } 00123 00124 int ORO_main(int argc, char **argv){ 00125 testing::InitGoogleTest(&argc,argv); 00126 return RUN_ALL_TESTS(); 00127 } 00128