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 
00054   bool checkModel(urdf::Model & robot)
00055   {
00056     // get root link
00057     urdf::LinkConstSharedPtr root_link = robot.getRoot();
00058     if (!root_link)
00059     {
00060       ROS_ERROR("no root link %s", robot.getName().c_str());
00061       return false;
00062     }
00063 
00064     // go through entire tree
00065     return this->traverse_tree(root_link);
00066 
00067   };
00068 
00069 protected:
00071   // num_links starts at 1 because traverse_tree doesn't count the root node
00072   TestParser() : num_joints(0), num_links(1)
00073   {
00074   }
00075 
00076 
00078   ~TestParser()
00079   {
00080   }
00081 
00082   bool traverse_tree(urdf::LinkConstSharedPtr link,int level = 0)
00083   {
00084     ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size());
00085     level+=2;
00086     bool retval = true;
00087     for (std::vector<urdf::LinkSharedPtr>::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
00088     {
00089       ++num_links;
00090       if (*child && (*child)->parent_joint)
00091       {
00092         ++num_joints;
00093         // check rpy
00094         double roll,pitch,yaw;
00095         (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw);
00096 
00097         if (isnan(roll) || isnan(pitch) || isnan(yaw))
00098         {
00099           ROS_ERROR("getRPY() returned nan!");
00100           return false;
00101         }
00102         // recurse down the tree
00103         retval &= this->traverse_tree(*child,level);
00104       }
00105       else
00106       {
00107         ROS_ERROR("root link: %s has a null child!",link->name.c_str());
00108         return false;
00109       }
00110     }
00111     // no more children
00112     return retval;
00113   };
00114 
00115   size_t num_joints;
00116   size_t num_links;
00117 };
00118 
00119 
00120 
00121 
00122 TEST_F(TestParser, test)
00123 {
00124   ASSERT_GE(g_argc, 3);
00125   std::string folder = std::string(g_argv[1]) + "/test/";
00126   ROS_INFO("Folder %s",folder.c_str());
00127   std::string file = std::string(g_argv[2]);
00128   bool expect_success = (file.substr(0,5)  != "fail_");
00129   urdf::Model robot;
00130   ROS_INFO("Parsing file %s, expecting %d",(folder + file).c_str(), expect_success);
00131   if (!expect_success) {
00132     ASSERT_FALSE(robot.initFile(folder + file));
00133     return;
00134   }
00135 
00136   ASSERT_EQ(g_argc, 7);
00137   std::string robot_name = std::string(g_argv[3]);
00138   std::string root_name = std::string(g_argv[4]);
00139   size_t expected_num_joints = atoi(g_argv[5]);
00140   size_t expected_num_links = atoi(g_argv[6]);
00141 
00142   ASSERT_TRUE(robot.initFile(folder + file));
00143 
00144   EXPECT_EQ(robot.getName(), robot_name);
00145   urdf::LinkConstSharedPtr root = robot.getRoot();
00146   ASSERT_TRUE(static_cast<bool>(root));
00147   EXPECT_EQ(root->name, root_name);
00148 
00149   ASSERT_TRUE(checkModel(robot));
00150   EXPECT_EQ(num_joints, expected_num_joints);
00151   EXPECT_EQ(num_links, expected_num_links);
00152   EXPECT_EQ(robot.joints_.size(), expected_num_joints);
00153   EXPECT_EQ(robot.links_.size(), expected_num_links);
00154 
00155   // test reading from parameter server
00156   ASSERT_TRUE(robot.initParam("robot_description"));
00157   ASSERT_FALSE(robot.initParam("robot_description_wim"));
00158   SUCCEED();
00159 }
00160 
00161 
00162 
00163 
00164 int main(int argc, char** argv)
00165 {
00166   // Calling ros::init(), just to remove unwanted args from command-line.
00167   ros::init(argc, argv, "test", ros::init_options::AnonymousName);
00168   testing::InitGoogleTest(&argc, argv);
00169   g_argc = argc;
00170   g_argv = argv;
00171   return RUN_ALL_TESTS();
00172 }


urdf
Author(s): Ioan Sucan , Jackie Kay
autogenerated on Thu Jun 6 2019 17:38:34