test_robot_model_parser.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, 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: Wim Meeussen */
00036 
00037 #include <string>
00038 #include <gtest/gtest.h>
00039 #include "urdf/model.h"
00040 
00041 // Including ros, just to be able to call ros::init(), to remove unwanted
00042 // args from command-line.
00043 #include <ros/ros.h>
00044 
00045 using namespace urdf;
00046 
00047 int g_argc;
00048 char** g_argv;
00049 
00050 class TestParser : public testing::Test
00051 {
00052 public:
00053   Model robot;
00054 
00055   bool checkModel()
00056   {
00057     // get root link
00058     boost::shared_ptr<const Link> root_link=this->robot.getRoot();
00059     if (!root_link)
00060     {
00061       ROS_ERROR("no root link %s",this->robot.getName().c_str());
00062       return false;
00063     }
00064 
00065     // go through entire tree
00066     return this->traverse_tree(root_link); 
00067 
00068   };
00069 
00070 protected:
00072   TestParser()
00073   {
00074   }
00075 
00076 
00078   ~TestParser()
00079   {
00080   }
00081 
00082   bool traverse_tree(boost::shared_ptr<const Link> link,int level = 0)
00083   {
00084     level+=2;
00085     for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
00086     {
00087       if (*child)
00088       {
00089         // check rpy
00090         double roll,pitch,yaw;
00091         (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw);
00092 
00093         if (isnan(roll) || isnan(pitch) || isnan(yaw))
00094         {
00095           ROS_ERROR("getRPY() returned nan!");
00096           return false;
00097         }
00098         // recurse down the tree
00099         return this->traverse_tree(*child,level);
00100       }
00101       else
00102       {
00103         ROS_ERROR("root link: %s has a null child!",link->name.c_str());
00104         return false;
00105       }
00106     }
00107     // no children
00108     return true;
00109   };
00110 };
00111 
00112 
00113 
00114 
00115 TEST_F(TestParser, test)
00116 {
00117   std::string folder = std::string(g_argv[1]) + "/test/";
00118   ROS_INFO("Folder %s",folder.c_str());
00119   for (int i=2; i<g_argc; i++){
00120     std::string file = g_argv[i];
00121     bool expect_success = (file.substr(0,5)  != "fail_");
00122     ROS_INFO("Parsing file %d/%d called %s, expecting %d",(i-1), g_argc-1, (folder + file).c_str(), expect_success);
00123     if (!expect_success)
00124       ASSERT_FALSE(robot.initFile(folder + file));
00125     else
00126     {
00127       ASSERT_TRUE(robot.initFile(folder + file));
00128       ASSERT_TRUE(checkModel());
00129     }
00130   }
00131 
00132   // test reading from parameter server
00133   ASSERT_TRUE(robot.initParam("robot_description"));
00134   ASSERT_FALSE(robot.initParam("robot_description_wim"));
00135   SUCCEED();
00136 }
00137 
00138 
00139 
00140 
00141 int main(int argc, char** argv)
00142 {
00143   // Calling ros::init(), just to remove unwanted args from command-line.
00144   ros::init(argc, argv, "test", ros::init_options::AnonymousName);
00145   testing::InitGoogleTest(&argc, argv);
00146   g_argc = argc;
00147   g_argv = argv;
00148   return RUN_ALL_TESTS();
00149 }


urdf
Author(s): Ioan Sucan
autogenerated on Thu Aug 27 2015 14:41:00