test_parser.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 the 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 <srdfdom/model.h>
00038 #include <urdf_parser/urdf_parser.h>
00039 #include <fstream>
00040 #include <stdexcept>
00041 #include <boost/lexical_cast.hpp>
00042 
00043 namespace urdf
00044 {
00045 typedef boost::shared_ptr<const ::urdf::ModelInterface> ModelInterfaceSharedPtr;
00046 }
00047 
00048 #define EXPECT_TRUE(arg) if (!(arg)) throw std::runtime_error("Assertion failed at line " + boost::lexical_cast<std::string>(__LINE__))
00049 
00050 #ifndef TEST_RESOURCE_LOCATION
00051 #  define TEST_RESOURCE_LOCATION "."
00052 #endif
00053 
00054 urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename)
00055 {
00056   // get the entire file
00057   std::string xml_string;
00058   std::fstream xml_file(filename.c_str(), std::fstream::in);
00059   if (xml_file.is_open())
00060   {
00061     while (xml_file.good())
00062     {
00063       std::string line;
00064       std::getline( xml_file, line);
00065       xml_string += (line + "\n");
00066     }
00067     xml_file.close();
00068     return urdf::parseURDF(xml_string);
00069   }
00070   else
00071   {
00072     throw std::runtime_error("Could not open file " + filename + " for parsing.");
00073     return urdf::ModelInterfaceSharedPtr();
00074   }
00075 }
00076 
00077 void testSimple(void)
00078 {
00079   srdf::Model s;
00080   urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
00081   EXPECT_TRUE(u != NULL);
00082   
00083   EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
00084   EXPECT_TRUE(s.getVirtualJoints().size() == 0);
00085   EXPECT_TRUE(s.getGroups().size() == 0);
00086   EXPECT_TRUE(s.getGroupStates().size() == 0);
00087   EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
00088   EXPECT_TRUE(s.getEndEffectors().size() == 0);
00089   
00090   EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.2.srdf"));
00091   EXPECT_TRUE(s.getVirtualJoints().size() == 1);
00092   EXPECT_TRUE(s.getGroups().size() == 1);
00093   EXPECT_TRUE(s.getGroupStates().size() == 0);
00094   EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
00095   EXPECT_TRUE(s.getEndEffectors().size() == 0);
00096   
00097   EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
00098   EXPECT_TRUE(s.getVirtualJoints().size() == 0);
00099   EXPECT_TRUE(s.getGroups().size() == 0);
00100   EXPECT_TRUE(s.getGroupStates().size() == 0);
00101   EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
00102   EXPECT_TRUE(s.getEndEffectors().size() == 0);
00103 }
00104 
00105 void testComplex(void)
00106 {
00107   srdf::Model s;
00108   urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
00109   EXPECT_TRUE(u != NULL);
00110 
00111   EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.3.srdf"));
00112   EXPECT_TRUE(s.getVirtualJoints().size() == 1);
00113   EXPECT_TRUE(s.getGroups().size() == 7);
00114   EXPECT_TRUE(s.getGroupStates().size() == 2);
00115   EXPECT_TRUE(s.getDisabledCollisionPairs().size() == 2);
00116   EXPECT_TRUE(s.getDisabledCollisionPairs()[0].reason_ == "adjacent");
00117   EXPECT_TRUE(s.getEndEffectors().size() == 2);
00118   
00119   EXPECT_TRUE(s.getVirtualJoints()[0].name_ == "world_joint");
00120   EXPECT_TRUE(s.getVirtualJoints()[0].type_ == "planar");
00121   for (std::size_t i = 0 ; i < s.getGroups().size() ; ++i)
00122   {
00123     if (s.getGroups()[i].name_ == "left_arm" || s.getGroups()[i].name_ == "right_arm")
00124       EXPECT_TRUE(s.getGroups()[i].chains_.size() == 1);
00125     if (s.getGroups()[i].name_ == "arms")
00126       EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
00127     if (s.getGroups()[i].name_ == "base")
00128       EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
00129     if (s.getGroups()[i].name_ == "l_end_effector" || s.getGroups()[i].name_ == "r_end_effector")
00130     {
00131       EXPECT_TRUE(s.getGroups()[i].links_.size() == 1);
00132       EXPECT_TRUE(s.getGroups()[i].joints_.size() == 9);
00133     }
00134     if (s.getGroups()[i].name_ == "whole_body")
00135     {
00136       EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
00137       EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
00138     }
00139   }
00140   int index = 0;
00141   if (s.getGroupStates()[0].group_ != "arms")
00142     index = 1;
00143   
00144   EXPECT_TRUE(s.getGroupStates()[index].group_ == "arms");
00145   EXPECT_TRUE(s.getGroupStates()[index].name_ == "tuck_arms");
00146   EXPECT_TRUE(s.getGroupStates()[1-index].group_ == "base");
00147   EXPECT_TRUE(s.getGroupStates()[1-index].name_ == "home");
00148   
00149   const std::vector<double> &v = s.getGroupStates()[index].joint_values_.find("l_shoulder_pan_joint")->second;
00150   EXPECT_TRUE(v.size() == 1u);
00151   EXPECT_TRUE(v[0] == 0.2);
00152   const std::vector<double> &w = s.getGroupStates()[1-index].joint_values_.find("world_joint")->second;
00153   EXPECT_TRUE(w.size() == 3u);
00154   EXPECT_TRUE(w[0] == 0.4);
00155   EXPECT_TRUE(w[1] == 0);
00156   EXPECT_TRUE(w[2] == -1);
00157   
00158   index = (s.getEndEffectors()[0].name_[0] == 'r') ? 0 : 1;
00159   EXPECT_TRUE(s.getEndEffectors()[index].name_ == "r_end_effector");
00160   EXPECT_TRUE(s.getEndEffectors()[index].component_group_ == "r_end_effector");
00161   EXPECT_TRUE(s.getEndEffectors()[index].parent_link_ == "r_wrist_roll_link");
00162 }
00163 
00164 int main(int argc, char **argv)
00165 {
00166   testSimple();
00167   testComplex();
00168   return 0;
00169 }


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Thu Feb 9 2017 03:50:57