Skip to content

Commit

Permalink
porting cpp tests to ros2
Browse files Browse the repository at this point in the history
  • Loading branch information
Michael Lautman committed Mar 14, 2019
1 parent 42624c3 commit 6079c12
Showing 1 changed file with 133 additions and 16 deletions.
149 changes: 133 additions & 16 deletions test/test_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,14 +36,12 @@

#include <srdfdom/model.h>
#include <urdf_parser/urdf_parser.h>
#include <ament_index_cpp/get_resource.hpp>

#include <fstream>
#include <stdexcept>
#include <gtest/gtest.h>

#ifndef TEST_RESOURCE_LOCATION
#define TEST_RESOURCE_LOCATION "."
#endif

struct ScopedLocale
{
ScopedLocale(const char* name = "C")
Expand All @@ -58,9 +56,9 @@ struct ScopedLocale
std::string backup_;
};

urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename)
urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename, const char* locale_name = "C")
{
ScopedLocale l("C");
ScopedLocale l(locale_name);
// get the entire file
std::string xml_string;
std::fstream xml_file(filename.c_str(), std::fstream::in);
Expand All @@ -82,41 +80,162 @@ urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename)
}
}

TEST(TestCpp, testSimple)
TEST(TestCpp, testSimpleC)
{
std::string package_name = "srdfdom";
srdf::Model s;
std::string content;
std::string prefix_path;
EXPECT_TRUE(ament_index_cpp::get_resource("packages", package_name, content, &prefix_path));
std::string test_resource_location = prefix_path + "/share/" + package_name + "/resources/";

setlocale(LC_ALL, "C");
std::cout << "Using locale: " << setlocale(LC_ALL, nullptr) << std::endl;
urdf::ModelInterfaceSharedPtr u = loadURDF(test_resource_location + "/pr2_desc.urdf", "C");
ASSERT_TRUE(u != NULL);

EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);

EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.2.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 1);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);

EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
}

TEST(TestCpp, testSimpleUTF)
{
std::string package_name = "srdfdom";
srdf::Model s;
urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");
std::string content;
std::string prefix_path;
EXPECT_TRUE(ament_index_cpp::get_resource("packages", package_name, content, &prefix_path));
std::string test_resource_location = prefix_path + "/share/" + package_name + "/resources/";

setlocale(LC_ALL, "nl_NL.UTF-8");
std::cout << "Using locale: " << setlocale(LC_ALL, nullptr) << std::endl;
urdf::ModelInterfaceSharedPtr u = loadURDF(test_resource_location + "/pr2_desc.urdf", "nl_NL.UTF-8");
ASSERT_TRUE(u != NULL);

EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);

EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.2.srdf"));
EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.2.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 1);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);

EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf"));
EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.1.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 0);
EXPECT_TRUE(s.getGroups().size() == 0);
EXPECT_TRUE(s.getGroupStates().size() == 0);
EXPECT_TRUE(s.getDisabledCollisionPairs().empty());
EXPECT_TRUE(s.getEndEffectors().size() == 0);
}

TEST(TestCpp, testComplex)
TEST(TestCpp, testComplexC)
{
srdf::Model s;

std::string package_name = "srdfdom";
std::string content;
std::string prefix_path;
EXPECT_TRUE(ament_index_cpp::get_resource("packages", package_name, content, &prefix_path));
std::string test_resource_location = prefix_path + "/share/" + package_name + "/resources/";

setlocale(LC_ALL, "C");
std::cout << "Using locale: " << setlocale(LC_ALL, nullptr) << std::endl;
urdf::ModelInterfaceSharedPtr u = loadURDF(test_resource_location + "/pr2_desc.urdf", "C");
EXPECT_TRUE(u != NULL);

EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.3.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 7);
EXPECT_TRUE(s.getGroupStates().size() == 2);
EXPECT_TRUE(s.getDisabledCollisionPairs().size() == 2);
EXPECT_TRUE(s.getDisabledCollisionPairs()[0].reason_ == "adjacent");
EXPECT_TRUE(s.getEndEffectors().size() == 2);

EXPECT_TRUE(s.getVirtualJoints()[0].name_ == "world_joint");
EXPECT_TRUE(s.getVirtualJoints()[0].type_ == "planar");
for (std::size_t i = 0; i < s.getGroups().size(); ++i)
{
if (s.getGroups()[i].name_ == "left_arm" || s.getGroups()[i].name_ == "right_arm")
EXPECT_TRUE(s.getGroups()[i].chains_.size() == 1);
if (s.getGroups()[i].name_ == "arms")
EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
if (s.getGroups()[i].name_ == "base")
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
if (s.getGroups()[i].name_ == "l_end_effector" || s.getGroups()[i].name_ == "r_end_effector")
{
EXPECT_TRUE(s.getGroups()[i].links_.size() == 1);
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 9);
}
if (s.getGroups()[i].name_ == "whole_body")
{
EXPECT_TRUE(s.getGroups()[i].joints_.size() == 1);
EXPECT_TRUE(s.getGroups()[i].subgroups_.size() == 2);
}
}
int index = 0;
if (s.getGroupStates()[0].group_ != "arms")
index = 1;

EXPECT_TRUE(s.getGroupStates()[index].group_ == "arms");
EXPECT_TRUE(s.getGroupStates()[index].name_ == "tuck_arms");
EXPECT_TRUE(s.getGroupStates()[1 - index].group_ == "base");
EXPECT_TRUE(s.getGroupStates()[1 - index].name_ == "home");

const std::vector<double>& v = s.getGroupStates()[index].joint_values_.find("l_shoulder_pan_joint")->second;
EXPECT_EQ(v.size(), 1u);
EXPECT_EQ(v[0], 0.2);
const std::vector<double>& w = s.getGroupStates()[1 - index].joint_values_.find("world_joint")->second;
EXPECT_EQ(w.size(), 3u);
EXPECT_EQ(w[0], 0.4);
EXPECT_EQ(w[1], 0);
EXPECT_EQ(w[2], -1);

index = (s.getEndEffectors()[0].name_[0] == 'r') ? 0 : 1;
EXPECT_TRUE(s.getEndEffectors()[index].name_ == "r_end_effector");
EXPECT_TRUE(s.getEndEffectors()[index].component_group_ == "r_end_effector");
EXPECT_TRUE(s.getEndEffectors()[index].parent_link_ == "r_wrist_roll_link");
}

TEST(TestCpp, testComplexUTF)
{
srdf::Model s;
urdf::ModelInterfaceSharedPtr u = loadURDF(std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.urdf");

std::string package_name = "srdfdom";
std::string content;
std::string prefix_path;
EXPECT_TRUE(ament_index_cpp::get_resource("packages", package_name, content, &prefix_path));
std::string test_resource_location = prefix_path + "/share/" + package_name + "/resources/";

setlocale(LC_ALL, "nl_NL.UTF-8");
std::cout << "Using locale: " << setlocale(LC_ALL, nullptr) << std::endl;
urdf::ModelInterfaceSharedPtr u = loadURDF(test_resource_location + "/pr2_desc.urdf", "nl_NL.UTF-8");
EXPECT_TRUE(u != NULL);

EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.3.srdf"));
EXPECT_TRUE(s.initFile(*u, test_resource_location + "/pr2_desc.3.srdf"));
EXPECT_TRUE(s.getVirtualJoints().size() == 1);
EXPECT_TRUE(s.getGroups().size() == 7);
EXPECT_TRUE(s.getGroupStates().size() == 2);
Expand Down Expand Up @@ -172,8 +291,6 @@ TEST(TestCpp, testComplex)
int main(int argc, char** argv)
{
// use the environment locale so that the unit test can be repeated with various locales easily
setlocale(LC_ALL, "");
std::cout << "Using locale: " << setlocale(LC_ALL, nullptr) << std::endl;

testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
Expand Down

0 comments on commit 6079c12

Please sign in to comment.