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
00035
00036
00037 #include <moveit/robot_model/robot_model.h>
00038 #include <urdf_parser/urdf_parser.h>
00039 #include <fstream>
00040 #include <gtest/gtest.h>
00041 #include <boost/filesystem/path.hpp>
00042 #include <moveit/profiler/profiler.h>
00043 #include <moveit_resources/config.h>
00044
00045 class LoadPlanningModelsPr2 : public testing::Test
00046 {
00047 protected:
00048 virtual void SetUp()
00049 {
00050 boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
00051
00052 srdf_model.reset(new srdf::Model());
00053 std::string xml_string;
00054 std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
00055 if (xml_file.is_open())
00056 {
00057 while (xml_file.good())
00058 {
00059 std::string line;
00060 std::getline(xml_file, line);
00061 xml_string += (line + "\n");
00062 }
00063 xml_file.close();
00064 urdf_model = urdf::parseURDF(xml_string);
00065 }
00066 srdf_model->initFile(*urdf_model, (res_path / "pr2_description/srdf/robot.xml").string());
00067 robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
00068 };
00069
00070 virtual void TearDown()
00071 {
00072 }
00073
00074 protected:
00075 boost::shared_ptr<urdf::ModelInterface> urdf_model;
00076 boost::shared_ptr<srdf::Model> srdf_model;
00077 moveit::core::RobotModelConstPtr robot_model;
00078 };
00079
00080 TEST_F(LoadPlanningModelsPr2, InitOK)
00081 {
00082 ASSERT_EQ(urdf_model->getName(), "pr2");
00083 ASSERT_EQ(srdf_model->getName(), "pr2");
00084 }
00085
00086 TEST_F(LoadPlanningModelsPr2, Model)
00087 {
00088
00089
00090 const std::vector<const moveit::core::JointModel*>& joints = robot_model->getJointModels();
00091 for (std::size_t i = 0; i < joints.size(); ++i)
00092 {
00093 ASSERT_EQ(joints[i]->getJointIndex(), i);
00094 ASSERT_EQ(robot_model->getJointModel(joints[i]->getName()), joints[i]);
00095 }
00096 const std::vector<const moveit::core::LinkModel*>& links = robot_model->getLinkModels();
00097 for (std::size_t i = 0; i < links.size(); ++i)
00098 {
00099 ASSERT_EQ(links[i]->getLinkIndex(), i);
00100
00101 }
00102 moveit::tools::Profiler::Status();
00103 }
00104
00105 int main(int argc, char** argv)
00106 {
00107 testing::InitGoogleTest(&argc, argv);
00108 return RUN_ALL_TESTS();
00109 }