Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include <moveit/robot_model/robot_model.h>
00038 #include <moveit/robot_state/robot_state.h>
00039 #include <moveit/robot_state/transforms.h>
00040 #include <urdf_parser/urdf_parser.h>
00041 #include <fstream>
00042 #include <gtest/gtest.h>
00043
00044 class LoadPlanningModelsPr2 : public testing::Test
00045 {
00046 protected:
00047
00048 virtual void SetUp()
00049 {
00050 srdf_model_.reset(new srdf::Model());
00051
00052 std::string xml_string;
00053 std::fstream xml_file("test/urdf/robot.xml", std::fstream::in);
00054 if (xml_file.is_open())
00055 {
00056 while ( xml_file.good() )
00057 {
00058 std::string line;
00059 std::getline( xml_file, line);
00060 xml_string += (line + "\n");
00061 }
00062 xml_file.close();
00063 urdf_model_ = urdf::parseURDF(xml_string);
00064 urdf_ok_ = urdf_model_;
00065 }
00066 else
00067 urdf_ok_ = false;
00068 srdf_ok_ = srdf_model_->initFile(*urdf_model_, "test/srdf/robot.xml");
00069 };
00070
00071 virtual void TearDown()
00072 {
00073 }
00074
00075 protected:
00076
00077 boost::shared_ptr<urdf::ModelInterface> urdf_model_;
00078 boost::shared_ptr<srdf::Model> srdf_model_;
00079 bool urdf_ok_;
00080 bool srdf_ok_;
00081
00082 };
00083
00084 TEST_F(LoadPlanningModelsPr2, InitOK)
00085 {
00086 ASSERT_TRUE(urdf_ok_);
00087 ASSERT_EQ(urdf_model_->getName(), "pr2_test");
00088
00089 robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
00090 robot_state::RobotState ks(kmodel);
00091 ks.setToRandomValues();
00092 ks.setToDefaultValues();
00093
00094
00095 robot_state::Transforms tf(kmodel->getModelFrame());
00096
00097 Eigen::Affine3d t1;
00098 t1.setIdentity();
00099 t1.translation() = Eigen::Vector3d(10.0, 1.0, 0.0);
00100 tf.setTransform(t1, "some_frame_1");
00101
00102 Eigen::Affine3d t2(Eigen::Translation3d(10.0, 1.0, 0.0)*Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY()));
00103 tf.setTransform(t2, "some_frame_2");
00104
00105 Eigen::Affine3d t3;
00106 t3.setIdentity();
00107 t3.translation() = Eigen::Vector3d(0.0, 1.0, -1.0);
00108 tf.setTransform(t3, "some_frame_3");
00109
00110
00111 EXPECT_TRUE(tf.isFixedFrame("some_frame_1"));
00112 EXPECT_FALSE(tf.isFixedFrame("base_footprint"));
00113 EXPECT_TRUE(tf.isFixedFrame(kmodel->getModelFrame()));
00114
00115 Eigen::Affine3d x;
00116 x.setIdentity();
00117 tf.transformPose(ks, "some_frame_2", x, x);
00118
00119 EXPECT_TRUE(t2.translation() == x.translation());
00120 EXPECT_TRUE(t2.rotation() == x.rotation());
00121
00122 tf.transformPose(ks, kmodel->getModelFrame(), x, x);
00123 EXPECT_TRUE(t2.translation() == x.translation());
00124 EXPECT_TRUE(t2.rotation() == x.rotation());
00125
00126 x.setIdentity();
00127 tf.transformPose(ks, "r_wrist_roll_link", x, x);
00128
00129 EXPECT_NEAR(x.translation().x(), 0.585315, 1e-4);
00130 EXPECT_NEAR(x.translation().y(), -0.188, 1e-4);
00131 EXPECT_NEAR(x.translation().z(), 1.24001, 1e-4);
00132 }
00133
00134
00135 int main(int argc, char **argv)
00136 {
00137 testing::InitGoogleTest(&argc, argv);
00138 return RUN_ALL_TESTS();
00139 }