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/rdf_loader/rdf_loader.h>
00038 #include <moveit/profiler/profiler.h>
00039 #include <ros/ros.h>
00040
00041 rdf_loader::RDFLoader::RDFLoader(const std::string& robot_description)
00042 {
00043 moveit::tools::Profiler::ScopedStart prof_start;
00044 moveit::tools::Profiler::ScopedBlock prof_block("RDFLoader(robot_description)");
00045
00046 ros::WallTime start = ros::WallTime::now();
00047 ros::NodeHandle nh("~");
00048 std::string content;
00049
00050 if (!nh.searchParam(robot_description, robot_description_) || !nh.getParam(robot_description_, content))
00051 {
00052 ROS_ERROR("Robot model parameter not found! Did you remap '%s'?", robot_description.c_str());
00053 return;
00054 }
00055
00056 urdf::Model* umodel = new urdf::Model();
00057 if (!umodel->initString(content))
00058 {
00059 ROS_ERROR("Unable to parse URDF from parameter '%s'", robot_description_.c_str());
00060 return;
00061 }
00062 urdf_.reset(umodel);
00063
00064 const std::string srdf_description(robot_description_ + "_semantic");
00065 std::string scontent;
00066 if (!nh.getParam(srdf_description, scontent))
00067 {
00068 ROS_ERROR("Robot semantic description not found. Did you forget to define or remap '%s'?",
00069 srdf_description.c_str());
00070 return;
00071 }
00072
00073 srdf_.reset(new srdf::Model());
00074 if (!srdf_->initString(*urdf_, scontent))
00075 {
00076 ROS_ERROR("Unable to parse SRDF from parameter '%s'", srdf_description.c_str());
00077 srdf_.reset();
00078 return;
00079 }
00080
00081 ROS_DEBUG_STREAM_NAMED("rdf", "Loaded robot model in " << (ros::WallTime::now() - start).toSec() << " seconds");
00082 }
00083
00084 rdf_loader::RDFLoader::RDFLoader(const std::string& urdf_string, const std::string& srdf_string)
00085 {
00086 moveit::tools::Profiler::ScopedStart prof_start;
00087 moveit::tools::Profiler::ScopedBlock prof_block("RDFLoader(string)");
00088
00089 urdf::Model* umodel = new urdf::Model();
00090 urdf_.reset(umodel);
00091 if (umodel->initString(urdf_string))
00092 {
00093 srdf_.reset(new srdf::Model());
00094 if (!srdf_->initString(*urdf_, srdf_string))
00095 {
00096 ROS_ERROR("Unable to parse SRDF");
00097 srdf_.reset();
00098 }
00099 }
00100 else
00101 {
00102 ROS_ERROR("Unable to parse URDF");
00103 urdf_.reset();
00104 }
00105 }
00106
00107 rdf_loader::RDFLoader::RDFLoader(TiXmlDocument* urdf_doc, TiXmlDocument* srdf_doc)
00108 {
00109 moveit::tools::Profiler::ScopedStart prof_start;
00110 moveit::tools::Profiler::ScopedBlock prof_block("RDFLoader(XML)");
00111
00112 urdf::Model* umodel = new urdf::Model();
00113 urdf_.reset(umodel);
00114 if (umodel->initXml(urdf_doc))
00115 {
00116 srdf_.reset(new srdf::Model());
00117 if (!srdf_->initXml(*urdf_, srdf_doc))
00118 {
00119 ROS_ERROR("Unable to parse SRDF");
00120 srdf_.reset();
00121 }
00122 }
00123 else
00124 {
00125 ROS_ERROR("Unable to parse URDF");
00126 urdf_.reset();
00127 }
00128 }