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