Go to the documentation of this file.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 <kdl/frames_io.hpp>
00011 #include <rtt/os/main.h>
00012
00013 class KDLPluginTest : public testing::Test {
00014
00015 #define COMPNAME "Deployer"
00016
00017 protected:
00018 KDLPluginTest():
00019 depl(COMPNAME),
00020 chain_prop("chain",""),
00021 frame_prop("frame",""),
00022 jacobian_prop("jacobian",""),
00023 jntarray_prop("jntarray",""),
00024 joint_prop("joint",""),
00025 rotation_prop("rotation",""),
00026 segment_prop("segment",""),
00027 twist_prop("twist",""),
00028 vector_prop("vector",""),
00029 wrench_prop("wrench","")
00030 {}
00031
00032 virtual void SetUp(){
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
00079
00080
00081
00082
00083
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
00107
00108 EXPECT_TRUE(depl.addProperty(frame_prop));
00109
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 KDL::random(frame);
00119 KDL::random(twist);
00120 KDL::random(vector);
00121 KDL::random(wrench);
00122 KDL::random(rotation);
00123 jntarray.resize(3);
00124 KDL::random(jntarray(0));
00125 KDL::random(jntarray(1));
00126 KDL::random(jntarray(2));
00127 frame_prop.value()=frame;
00128 twist_prop.value()=twist;
00129 vector_prop.value()=vector;
00130 wrench_prop.value()=wrench;
00131 rotation_prop.value()=rotation;
00132 jntarray_prop.value()=jntarray;
00133
00134 EXPECT_TRUE(marsh->storeProperties("test.cpf"));
00135 EXPECT_TRUE(marsh->writeProperties("test1.cpf"));
00136 EXPECT_TRUE(marsh->updateFile("test1.cpf"));
00137 EXPECT_TRUE(marsh->readProperties("test.cpf"));
00138
00139 EXPECT_EQ(frame_prop.value(),frame);
00140 EXPECT_EQ(twist_prop.value(),twist);
00141 EXPECT_EQ(vector_prop.value(),vector);
00142 EXPECT_EQ(wrench_prop.value(),wrench);
00143 EXPECT_EQ(rotation_prop.value(),rotation);
00144 EXPECT_EQ(jntarray_prop.value(),jntarray);
00145 }
00146
00147 int ORO_main(int argc, char **argv){
00148 testing::InitGoogleTest(&argc,argv);
00149 return RUN_ALL_TESTS();
00150 }
00151