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