test_parser.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, 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: Ioan Sucan */
36 
37 #include <srdfdom/model.h>
38 #include <urdf_parser/urdf_parser.h>
39 #include <fstream>
40 #include <stdexcept>
41 #include <boost/lexical_cast.hpp>
42 
43 #define EXPECT_TRUE(arg) if (!(arg)) throw std::runtime_error("Assertion failed at line " + boost::lexical_cast<std::string>(__LINE__))
44 
45 #ifndef TEST_RESOURCE_LOCATION
46 # define TEST_RESOURCE_LOCATION "."
47 #endif
48 
49 urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename)
50 {
51  // get the entire file
52  std::string xml_string;
53  std::fstream xml_file(filename.c_str(), std::fstream::in);
54  if (xml_file.is_open())
55  {
56  while (xml_file.good())
57  {
58  std::string line;
59  std::getline( xml_file, line);
60  xml_string += (line + "\n");
61  }
62  xml_file.close();
63  return urdf::parseURDF(xml_string);
64  }
65  else
66  {
67  throw std::runtime_error("Could not open file " + filename + " for parsing.");
68  return urdf::ModelInterfaceSharedPtr();
69  }
70 }
71 
72 void testSimple(void)
73 {
74  srdf::Model s;
75  urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
76  EXPECT_TRUE(u != NULL);
77 
78  EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
79  EXPECT_TRUE(s.getVirtualJoints().size() == 0);
80  EXPECT_TRUE(s.getGroups().size() == 0);
81  EXPECT_TRUE(s.getGroupStates().size() == 0);
83  EXPECT_TRUE(s.getEndEffectors().size() == 0);
84 
85  EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.2.srdf"));
86  EXPECT_TRUE(s.getVirtualJoints().size() == 1);
87  EXPECT_TRUE(s.getGroups().size() == 1);
88  EXPECT_TRUE(s.getGroupStates().size() == 0);
90  EXPECT_TRUE(s.getEndEffectors().size() == 0);
91 
92  EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
93  EXPECT_TRUE(s.getVirtualJoints().size() == 0);
94  EXPECT_TRUE(s.getGroups().size() == 0);
95  EXPECT_TRUE(s.getGroupStates().size() == 0);
97  EXPECT_TRUE(s.getEndEffectors().size() == 0);
98 }
99 
100 void testComplex(void)
101 {
102  srdf::Model s;
103  urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
104  EXPECT_TRUE(u != NULL);
105 
106  EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.3.srdf"));
107  EXPECT_TRUE(s.getVirtualJoints().size() == 1);
108  EXPECT_TRUE(s.getGroups().size() == 7);
109  EXPECT_TRUE(s.getGroupStates().size() == 2);
110  EXPECT_TRUE(s.getDisabledCollisionPairs().size() == 2);
111  EXPECT_TRUE(s.getDisabledCollisionPairs()[0].reason_ == "adjacent");
112  EXPECT_TRUE(s.getEndEffectors().size() == 2);
113 
114  EXPECT_TRUE(s.getVirtualJoints()[0].name_ == "world_joint");
115  EXPECT_TRUE(s.getVirtualJoints()[0].type_ == "planar");
116  for (std::size_t i = 0 ; i < s.getGroups().size() ; ++i)
117  {
118  if (s.getGroups()[i].name_ == "left_arm" || s.getGroups()[i].name_ == "right_arm")
119  EXPECT_TRUE(s.getGroups()[i].chains_.size() == 1);
120  if (s.getGroups()[i].name_ == "arms")
121  EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
122  if (s.getGroups()[i].name_ == "base")
123  EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
124  if (s.getGroups()[i].name_ == "l_end_effector" || s.getGroups()[i].name_ == "r_end_effector")
125  {
126  EXPECT_TRUE(s.getGroups()[i].links_.size() == 1);
127  EXPECT_TRUE(s.getGroups()[i].joints_.size() == 9);
128  }
129  if (s.getGroups()[i].name_ == "whole_body")
130  {
131  EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
132  EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
133  }
134  }
135  int index = 0;
136  if (s.getGroupStates()[0].group_ != "arms")
137  index = 1;
138 
139  EXPECT_TRUE(s.getGroupStates()[index].group_ == "arms");
140  EXPECT_TRUE(s.getGroupStates()[index].name_ == "tuck_arms");
141  EXPECT_TRUE(s.getGroupStates()[1-index].group_ == "base");
142  EXPECT_TRUE(s.getGroupStates()[1-index].name_ == "home");
143 
144  const std::vector<double> &v = s.getGroupStates()[index].joint_values_.find("l_shoulder_pan_joint")->second;
145  EXPECT_TRUE(v.size() == 1u);
146  EXPECT_TRUE(v[0] == 0.2);
147  const std::vector<double> &w = s.getGroupStates()[1-index].joint_values_.find("world_joint")->second;
148  EXPECT_TRUE(w.size() == 3u);
149  EXPECT_TRUE(w[0] == 0.4);
150  EXPECT_TRUE(w[1] == 0);
151  EXPECT_TRUE(w[2] == -1);
152 
153  index = (s.getEndEffectors()[0].name_[0] == 'r') ? 0 : 1;
154  EXPECT_TRUE(s.getEndEffectors()[index].name_ == "r_end_effector");
155  EXPECT_TRUE(s.getEndEffectors()[index].component_group_ == "r_end_effector");
156  EXPECT_TRUE(s.getEndEffectors()[index].parent_link_ == "r_wrist_roll_link");
157 }
158 
159 int main(int argc, char **argv)
160 {
161  testSimple();
162  testComplex();
163  return 0;
164 }
#define TEST_RESOURCE_LOCATION
Definition: test_parser.cpp:46
int main(int argc, char **argv)
Representation of semantic information about the robot.
Definition: model.h:53
const std::vector< VirtualJoint > & getVirtualJoints() const
Get the list of virtual joints defined for this model.
Definition: model.h:222
bool initFile(const urdf::ModelInterface &urdf_model, const std::string &filename)
Load Model given a filename.
Definition: model.cpp:596
void testComplex(void)
XmlRpcServer s
void testSimple(void)
Definition: test_parser.cpp:72
__attribute__((deprecated)) std const std::vector< Group > & getGroups() const
Get the list of groups defined for this model.
Definition: model.h:216
const std::vector< DisabledCollision > & getDisabledCollisionPairs() const
Get the list of pairs of links that need not be checked for collisions (because they can never touch ...
Definition: model.h:206
def xml_string(rootXml, addHeader=True)
#define EXPECT_TRUE(arg)
Definition: test_parser.cpp:43
urdf::ModelInterfaceSharedPtr loadURDF(const std::string &filename)
Definition: test_parser.cpp:49
const std::vector< GroupState > & getGroupStates() const
Get the list of group states defined for this model.
Definition: model.h:234
const std::vector< EndEffector > & getEndEffectors() const
Get the list of end effectors defined for this model.
Definition: model.h:228


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Mon Jun 10 2019 15:06:23