test_robot_model_parser.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Wim Meeussen */
36 
37 #include <cmath>
38 #include <string>
39 #include <vector>
40 
41 #include <gtest/gtest.h>
42 #include "urdf/model.h"
43 
44 // Including ros, just to be able to call ros::init(), to remove unwanted
45 // args from command-line.
46 #include <ros/ros.h>
47 
48 int g_argc;
49 char ** g_argv;
50 
51 class TestParser : public testing::Test
52 {
53 public:
54  bool checkModel(urdf::Model & robot)
55  {
56  // get root link
57  urdf::LinkConstSharedPtr root_link = robot.getRoot();
58  if (!root_link) {
59  ROS_ERROR("no root link %s", robot.getName().c_str());
60  return false;
61  }
62 
63  // go through entire tree
64  return this->traverse_tree(root_link);
65  }
66 
67 protected:
69  // num_links starts at 1 because traverse_tree doesn't count the root node
71  : num_joints(0), num_links(1)
72  {
73  }
74 
77  {
78  }
79 
80  bool traverse_tree(urdf::LinkConstSharedPtr link, int level = 0)
81  {
82  ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size());
83  level += 2;
84  bool retval = true;
85  for (const urdf::LinkSharedPtr & child : link->child_links)
86  {
87  ++num_links;
88  if (child && child->parent_joint) {
89  ++num_joints;
90  // check rpy
91  double roll, pitch, yaw;
92  child->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll, pitch, yaw);
93 
94  if (std::isnan(roll) || std::isnan(pitch) || std::isnan(yaw)) {
95  ROS_ERROR("getRPY() returned nan!");
96  return false;
97  }
98  // recurse down the tree
99  retval &= this->traverse_tree(child, level);
100  } else {
101  ROS_ERROR("root link: %s has a null child!", link->name.c_str());
102  return false;
103  }
104  }
105  // no more children
106  return retval;
107  }
108 
109  size_t num_joints;
110  size_t num_links;
111 };
112 
114  ASSERT_GE(g_argc, 3);
115  std::string folder = std::string(g_argv[1]) + "/test/";
116  ROS_INFO("Folder %s", folder.c_str());
117  std::string file = std::string(g_argv[2]);
118  bool expect_success = (file.substr(0, 5) != "fail_");
119  urdf::Model robot;
120  ROS_INFO("Parsing file %s, expecting %d", (folder + file).c_str(), expect_success);
121  if (!expect_success) {
122  ASSERT_FALSE(robot.initFile(folder + file));
123  return;
124  }
125 
126  ASSERT_EQ(g_argc, 7);
127  std::string robot_name = std::string(g_argv[3]);
128  std::string root_name = std::string(g_argv[4]);
129  size_t expected_num_joints = atoi(g_argv[5]);
130  size_t expected_num_links = atoi(g_argv[6]);
131 
132  ASSERT_TRUE(robot.initFile(folder + file));
133 
134  EXPECT_EQ(robot.getName(), robot_name);
135  urdf::LinkConstSharedPtr root = robot.getRoot();
136  ASSERT_TRUE(static_cast<bool>(root));
137  EXPECT_EQ(root->name, root_name);
138 
139  ASSERT_TRUE(checkModel(robot));
140  EXPECT_EQ(num_joints, expected_num_joints);
141  EXPECT_EQ(num_links, expected_num_links);
142  EXPECT_EQ(robot.joints_.size(), expected_num_joints);
143  EXPECT_EQ(robot.links_.size(), expected_num_links);
144 
145  // test reading from parameter server
146  ASSERT_TRUE(robot.initParam("robot_description"));
147  ASSERT_FALSE(robot.initParam("robot_description_wim"));
148  SUCCEED();
149 }
150 
151 int main(int argc, char ** argv)
152 {
153  // Calling ros::init(), just to remove unwanted args from command-line.
154  ros::init(argc, argv, "test", ros::init_options::AnonymousName);
155  testing::InitGoogleTest(&argc, argv);
156  g_argc = argc;
157  g_argv = argv;
158  return RUN_ALL_TESTS();
159 }
ros::init_options::AnonymousName
AnonymousName
g_argc
int g_argc
Definition: test_robot_model_parser.cpp:48
model.h
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros.h
TestParser::~TestParser
~TestParser()
Destructor.
Definition: test_robot_model_parser.cpp:76
TestParser::num_joints
size_t num_joints
Definition: test_robot_model_parser.cpp:109
urdf::Model
Definition: model.h:89
TestParser::num_links
size_t num_links
Definition: test_robot_model_parser.cpp:110
TestParser::TestParser
TestParser()
constructor
Definition: test_robot_model_parser.cpp:70
urdf::Model::initFile
URDF_EXPORT bool initFile(const std::string &filename)
Load Model given a filename.
Definition: model.cpp:97
main
int main(int argc, char **argv)
Definition: test_robot_model_parser.cpp:151
g_argv
char ** g_argv
Definition: test_robot_model_parser.cpp:49
TEST_F
TEST_F(TestParser, test)
Definition: test_robot_model_parser.cpp:113
TestParser::traverse_tree
bool traverse_tree(urdf::LinkConstSharedPtr link, int level=0)
Definition: test_robot_model_parser.cpp:80
TestParser
Definition: test_robot_model_parser.cpp:51
TestParser::checkModel
bool checkModel(urdf::Model &robot)
Definition: test_robot_model_parser.cpp:54
urdf::Model::initParam
URDF_EXPORT bool initParam(const std::string &param)
Load Model given the name of a parameter on the parameter server.
Definition: model.cpp:117
ROS_ERROR
#define ROS_ERROR(...)
ROS_INFO
#define ROS_INFO(...)


urdf
Author(s): Ioan Sucan , Jackie Kay
autogenerated on Wed Jul 20 2022 02:43:43