rdf_loader.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:30