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 <srdfdom/srdf_writer.h>
39 #include <urdf_parser/urdf_parser.h>
40 #include <fstream>
41 #include <stdexcept>
42 #include <gtest/gtest.h>
43 
44 #ifndef TEST_RESOURCE_LOCATION
45 #define TEST_RESOURCE_LOCATION "."
46 #endif
47 
49 {
50  ScopedLocale(const char* name = "C")
51  {
52  backup_ = setlocale(LC_ALL, nullptr); // store current locale
53  setlocale(LC_ALL, name);
54  }
56  {
57  setlocale(LC_ALL, backup_.c_str()); // restore locale
58  }
59  std::string backup_;
60 };
61 
62 urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename)
63 {
64  ScopedLocale l("C");
65  // get the entire file
66  std::string xml_string;
67  std::fstream xml_file(filename.c_str(), std::fstream::in);
68  if (xml_file.is_open())
69  {
70  while (xml_file.good())
71  {
72  std::string line;
73  std::getline(xml_file, line);
74  xml_string += (line + "\n");
75  }
76  xml_file.close();
77  return urdf::parseURDF(xml_string);
78  }
79  else
80  {
81  throw std::runtime_error("Could not open file " + filename + " for parsing.");
82  return urdf::ModelInterfaceSharedPtr();
83  }
84 }
85 
86 TEST(TestCpp, testSimple)
87 {
88  srdf::Model s;
89  urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
90  ASSERT_TRUE(u != NULL);
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);
96  EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
97  EXPECT_TRUE(s.getEnabledCollisionPairs().empty());
98  EXPECT_TRUE(s.getEndEffectors().size() == 0);
99 
100  EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.2.srdf"));
101  EXPECT_TRUE(s.getVirtualJoints().size() == 1);
102  EXPECT_TRUE(s.getGroups().size() == 1);
103  EXPECT_TRUE(s.getGroupStates().size() == 0);
104  EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
105  EXPECT_TRUE(s.getEnabledCollisionPairs().empty());
106  EXPECT_TRUE(s.getEndEffectors().size() == 0);
107 
108  EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
109  EXPECT_TRUE(s.getVirtualJoints().size() == 0);
110  EXPECT_TRUE(s.getGroups().size() == 0);
111  EXPECT_TRUE(s.getGroupStates().size() == 0);
112  EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
113  EXPECT_TRUE(s.getEnabledCollisionPairs().empty());
114  EXPECT_TRUE(s.getEndEffectors().size() == 0);
115 }
116 
117 TEST(TestCpp, testComplex)
118 {
119  srdf::Model s;
120  urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
121  EXPECT_TRUE(u != NULL);
122 
123  EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.3.srdf"));
124  EXPECT_TRUE(s.getVirtualJoints().size() == 1);
125  EXPECT_TRUE(s.getGroups().size() == 7);
126  EXPECT_TRUE(s.getGroupStates().size() == 2);
127  EXPECT_TRUE(s.getDisabledCollisionPairs().size() == 2);
128  EXPECT_TRUE(s.getDisabledCollisionPairs()[0].reason_ == "adjacent");
129  EXPECT_TRUE(s.getEndEffectors().size() == 2);
130 
131  EXPECT_TRUE(s.getVirtualJoints()[0].name_ == "world_joint");
132  EXPECT_TRUE(s.getVirtualJoints()[0].type_ == "planar");
133  for (std::size_t i = 0; i < s.getGroups().size(); ++i)
134  {
135  if (s.getGroups()[i].name_ == "left_arm" || s.getGroups()[i].name_ == "right_arm")
136  {
137  EXPECT_TRUE(s.getGroups()[i].chains_.size() == 1);
138  }
139  if (s.getGroups()[i].name_ == "arms")
140  {
141  EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
142  }
143  if (s.getGroups()[i].name_ == "base")
144  {
145  EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
146  }
147  if (s.getGroups()[i].name_ == "l_end_effector" || s.getGroups()[i].name_ == "r_end_effector")
148  {
149  EXPECT_TRUE(s.getGroups()[i].links_.size() == 1);
150  EXPECT_TRUE(s.getGroups()[i].joints_.size() == 9);
151  }
152  if (s.getGroups()[i].name_ == "whole_body")
153  {
154  EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
155  EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
156  }
157  }
158  int index = 0;
159  if (s.getGroupStates()[0].group_ != "arms")
160  index = 1;
161 
162  EXPECT_TRUE(s.getGroupStates()[index].group_ == "arms");
163  EXPECT_TRUE(s.getGroupStates()[index].name_ == "tuck_arms");
164  EXPECT_TRUE(s.getGroupStates()[1 - index].group_ == "base");
165  EXPECT_TRUE(s.getGroupStates()[1 - index].name_ == "home");
166 
167  const std::vector<double>& v = s.getGroupStates()[index].joint_values_.find("l_shoulder_pan_joint")->second;
168  EXPECT_EQ(v.size(), 1u);
169  EXPECT_EQ(v[0], 0.2);
170  const std::vector<double>& w = s.getGroupStates()[1 - index].joint_values_.find("world_joint")->second;
171  EXPECT_EQ(w.size(), 3u);
172  EXPECT_EQ(w[0], 0.4);
173  EXPECT_EQ(w[1], 0);
174  EXPECT_EQ(w[2], -1);
175 
176  index = (s.getEndEffectors()[0].name_[0] == 'r') ? 0 : 1;
177  EXPECT_TRUE(s.getEndEffectors()[index].name_ == "r_end_effector");
178  EXPECT_TRUE(s.getEndEffectors()[index].component_group_ == "r_end_effector");
179  EXPECT_TRUE(s.getEndEffectors()[index].parent_link_ == "r_wrist_roll_link");
180 
181  // Joint Properties
182  const srdf::Model::PropertyMap& gripper_props = s.getJointProperties("r_gripper_tool_joint");
183  EXPECT_EQ(gripper_props.size(), 0u);
184 
185  // When parsing, this made up joint that is not present in the URDF is expected to print an error
186  // AND the property should not be made available in the srdf::Model
187  const srdf::Model::PropertyMap& made_up_props = s.getJointProperties("made_up_joint");
188  EXPECT_EQ(made_up_props.size(), 0u);
189 
190  const srdf::Model::PropertyMap& world_props = s.getJointProperties("world_joint");
191  ASSERT_EQ(world_props.size(), 1u);
192  EXPECT_EQ(world_props.at("angular_distance_weight"), "0.5");
193 }
194 
195 TEST(TestCpp, testReadWrite)
196 {
197  srdf::Model s;
198  urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
199  ASSERT_TRUE(u != nullptr);
200 
201  ASSERT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.3.srdf"));
202  srdf::SRDFWriter writer;
203  writer.initModel(*u, s);
204 
205  std::string filename = std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.3-normalized.srdf";
206  std::string xml_content;
207  std::fstream xml_file(filename.c_str(), std::fstream::in);
208  ASSERT_TRUE(xml_file.is_open());
209  while (xml_file.good())
210  {
211  std::string line;
212  std::getline(xml_file, line);
213  xml_content += (line + "\n");
214  }
215  xml_content.erase(xml_content.size() - 1, 1); // remove extra newline
216  xml_file.close();
217 
218  EXPECT_EQ(xml_content, writer.getSRDFString());
219 }
220 
221 int main(int argc, char** argv)
222 {
223  // use the environment locale so that the unit test can be repeated with various locales easily
224  setlocale(LC_ALL, "");
225  std::cout << "Using locale: " << setlocale(LC_ALL, nullptr) << std::endl;
226 
227  testing::InitGoogleTest(&argc, argv);
228  return RUN_ALL_TESTS();
229 }
ScopedLocale::ScopedLocale
ScopedLocale(const char *name="C")
Definition: test_parser.cpp:50
model.h
xml_string
def xml_string(rootXml, addHeader=True)
main
int main(int argc, char **argv)
Definition: test_parser.cpp:221
s
XmlRpcServer s
srdf::SRDFWriter
Definition: srdf_writer.h:83
srdf_writer.h
srdf::Model::PropertyMap
std::map< std::string, std::string > PropertyMap
Definition: model.h:119
TEST
TEST(TestCpp, testSimple)
Definition: test_parser.cpp:86
ScopedLocale::~ScopedLocale
~ScopedLocale()
Definition: test_parser.cpp:55
loadURDF
urdf::ModelInterfaceSharedPtr loadURDF(const std::string &filename)
Definition: test_parser.cpp:62
ScopedLocale
Definition: test_parser.cpp:48
ScopedLocale::backup_
std::string backup_
Definition: test_parser.cpp:59
srdf::SRDFWriter::initModel
void initModel(const urdf::ModelInterface &robot_model, const srdf::Model &srdf_model)
Definition: srdf_writer.cpp:92
srdf::SRDFWriter::getSRDFString
std::string getSRDFString()
Definition: srdf_writer.cpp:147
TEST_RESOURCE_LOCATION
#define TEST_RESOURCE_LOCATION
Definition: test_parser.cpp:45
srdf::Model
Representation of semantic information about the robot.
Definition: model.h:84


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Fri Jul 7 2023 02:22:48