diff --git a/.gitignore b/.gitignore index 378eac2..443cee8 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ build +__pycache__ +*.pyc diff --git a/.travis.yml b/.travis.yml index 3bd4e26..abf8557 100644 --- a/.travis.yml +++ b/.travis.yml @@ -15,14 +15,14 @@ env: global: # generate "exotic" locale for parser test - BEFORE_SCRIPT="sudo apt-get -qq install -y locales; sudo locale-gen nl_NL.UTF-8" - - ROS_DISTRO=melodic + - ROS_DISTRO=crystal - ROS_REPO=ros matrix: - - TEST="clang-format, catkin_lint" - - ROS_DISTRO=melodic + - TEST="clang-format" + - ROS_DISTRO=crystal before_script: - - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci + - git clone -q -b ros2 --depth=1 -b ros2 https://github.com/ros-planning/moveit_ci.git .moveit_ci script: - .moveit_ci/travis.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 6f61272..c0b1d7d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,66 +1,91 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(srdfdom) -find_package(Boost REQUIRED) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(console_bridge_vendor REQUIRED) # Provides console_bridge 0.4.0 on platforms without it. find_package(console_bridge REQUIRED) find_package(urdfdom_headers REQUIRED) +find_package(urdf REQUIRED) +find_package(tinyxml_vendor REQUIRED) +find_package(TinyXML REQUIRED) # provided by tinyxml_vendor +find_package(ament_index_cpp REQUIRED) -find_package(catkin REQUIRED COMPONENTS cmake_modules urdf urdfdom_py) - -find_package(TinyXML REQUIRED) +find_package(Boost REQUIRED) include_directories( include + ${rclcpp_INCLUDE_DIRS} + ${rmw_implementation_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${TinyXML_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS} - ) - -add_compile_options(-std=c++11) - -catkin_python_setup() - -catkin_package( - LIBRARIES ${PROJECT_NAME} - INCLUDE_DIRS include - DEPENDS TinyXML console_bridge urdfdom_headers + ${std_msgs_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME}) + +add_library(${PROJECT_NAME} src/model.cpp src/srdf_writer.cpp ) -target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${urdfdom_headers_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME} + ${TinyXML_LIBRARIES} + ${console_bridge_LIBRARIES} + ${urdfdom_headers_LIBRARIES} + ${urdf_LIBRARIES} +) -install(TARGETS ${PROJECT_NAME} - DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +install( + TARGETS + ${PROJECT_NAME} + DESTINATION lib ) + install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" + DESTINATION include/${PROJECT_NAME} ) -install(PROGRAMS +# Mark resources for installation +install(DIRECTORY test/resources + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS scripts/display_srdf - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION lib/${PROJECT_NAME} ) -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest(test/srdf_parser.test) +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + find_package(ament_cmake_gtest REQUIRED) - add_definitions(-DTEST_RESOURCE_LOCATION="${CMAKE_SOURCE_DIR}/test/resources") - execute_process(COMMAND bash -c "locale -a | grep -q ^nl_NL" - RESULT_VARIABLE TEST_LOCALE - OUTPUT_QUIET ERROR_QUIET) - if (TEST_LOCALE) - message(WARNING "Locale nl_NL not available. Locale test will not be meaningful.") - endif() + ament_add_gtest(test_parser test/test_parser.cpp) + target_link_libraries(test_parser + ${PROJECT_NAME} + ${rclcpp_LIBRARIES} + ${TinyXML_LIBRARIES} + ${console_bridge_LIBRARIES} + ${urdfdom_headers_LIBRARIES} + ${urdf_LIBRARIES} + ament_index_cpp::ament_index_cpp + ) - add_rostest_gtest(test_cpp test/srdf_parser_cpp.test test/test_parser.cpp) - target_link_libraries(test_cpp ${catkin_LIBRARIES} ${PROJECT_NAME}) + # python tests with python interfaces of message filters + find_package(ament_cmake_pytest REQUIRED) + ament_add_pytest_test(test.py "test/test.py" + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 90) endif() + +ament_package() \ No newline at end of file diff --git a/package.xml b/package.xml index 39494df..3c38a19 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,6 @@ - + + + srdfdom 0.5.1 Parser for Semantic Robot Description Format (SRDF). @@ -10,23 +12,32 @@ http://ros.org/wiki/srdfdom https://github.com/ros-planning/srdfdom/issues https://github.com/ros-planning/srdfdom - - catkin + + ament_cmake + + python_cmake_module + + console_bridge_vendor boost - cmake_modules + python_cmake_module libconsole-bridge-dev urdf liburdfdom-headers-dev urdfdom_py tinyxml - boost - libconsole-bridge-dev - liburdfdom-headers-dev - tinyxml - urdfdom_py + boost + libconsole-bridge-dev + liburdfdom-headers-dev + tinyxml + urdfdom_py - rostest + ament_index_cpp + ament_cmake_pytest + python3-pytest + + ament_cmake + diff --git a/src/srdfdom/srdf.py b/src/srdfdom/srdf.py index 7e9f5c1..4e97c40 100644 --- a/src/srdfdom/srdf.py +++ b/src/srdfdom/srdf.py @@ -49,16 +49,16 @@ def __init__(self, center = None, radius = 0.0): class VirtualJoint(xmlr.Object): TYPES = ['unknown', 'fixed', 'floating', 'planar'] - + def __init__(self, name = None, child_link = None, parent_frame = None, joint_type = None): self.name = name self.child_link = child_link self.parent_frame = parent_frame self.type = joint_type - + def check_valid(self): assert self.type in self.TYPES, "Invalid joint type: {}".format(self.type) - + # Aliases @property def joint_type(self): return self.type @@ -94,7 +94,7 @@ def __init__(self, name = None, group = None, parent_link = None, parent_group = name_attribute, xmlr.Attribute('group', str), xmlr.Attribute('parent_link', str), - xmlr.Attribute('parent_group', str, False) + xmlr.Attribute('parent_group', str, False) ]) class PassiveJoint(xmlr.Object): @@ -148,8 +148,8 @@ def __init__(self, name = None, group = None): xmlr.AggregateElement('joint', JointVal), xmlr.Attribute('group', str) ]) - - + + class LinkSphereApproximation(xmlr.Object): def __init__(self, link = None): self.aggregate_init() @@ -160,11 +160,11 @@ def __init__(self, link = None): xmlr.Attribute('link', str), xmlr.AggregateElement('sphere', Sphere) ]) - + class Robot(xmlr.Object): def __init__(self, name = None): self.aggregate_init() - + self.name = name self.groups = [] self.group_states = [] @@ -175,10 +175,10 @@ def __init__(self, name = None): self.link_sphere_approximations = [] self.group_map = {} self.group_state_map = {} - + def add_aggregate(self, typeName, elem): xmlr.Object.add_aggregate(self, typeName, elem) - + if typeName == 'group': group = elem self.group_map[group.name] = group @@ -191,22 +191,22 @@ def add_link(self, link): def add_joint(self, joint): self.add_aggregate('joint', joint) - + def add_chain(self, chain): self.add_aggregate('chain', chain) - + def add_group(self, group): self.add_aggregate('group', group) - + def add_passive_joint(self, joint): self.add_aggregate('passive_joint', joint) - + def add_disable_collisions(self, col): self.add_aggregate('disable_collisions', col) - + def add_link_sphere_approximation(self, link): self.add_aggregate('link_sphere_approximation', link) - + @classmethod def from_parameter_server(cls, key = 'robot_description_semantic'): @@ -219,7 +219,7 @@ def from_parameter_server(cls, key = 'robot_description_semantic'): # Could move this into xml_reflection import rospy return cls.from_xml_string(rospy.get_param(key)) - + xmlr.reflect(Robot, tag = 'robot', params = [ # name_attribute, xmlr.Attribute('name', str, False), # Is 'name' a required attribute? diff --git a/test/resources/pr2_desc.urdf b/test/resources/pr2_desc.urdf index cbc5c77..abd561b 100644 --- a/test/resources/pr2_desc.urdf +++ b/test/resources/pr2_desc.urdf @@ -73,10 +73,10 @@ diff --git a/test/srdf_parser.test b/test/srdf_parser.test deleted file mode 100644 index e40ee04..0000000 --- a/test/srdf_parser.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/test/srdf_parser_cpp.test b/test/srdf_parser_cpp.test deleted file mode 100644 index d7a3ce3..0000000 --- a/test/srdf_parser_cpp.test +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/test/test.py b/test/test.py index 4900bbe..7e0d723 100755 --- a/test/test.py +++ b/test/test.py @@ -1,14 +1,17 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 + PKG = 'srdfdom' +import os import sys -import rospkg import unittest from srdfdom.srdf import SRDF from xml.dom.minidom import parseString import xml.dom -# xml match code from test_xacro.py +from ament_index_python.resources import get_resource + +# xml match code from test_xacro.py # by Stuart Glaser and William Woodall def first_child_element(elt): @@ -18,7 +21,7 @@ def first_child_element(elt): return c c = c.nextSibling return None - + def next_sibling_element(elt): c = elt.nextSibling while c: @@ -85,39 +88,40 @@ def xml_matches(a, b): b.writexml(sys.stdout) return False return True - + ## A python unit test for srdf class TestSRDFParser(unittest.TestCase): - ## test valid srdf + ## test valid srdf def test_full_srdf(self): - srdf_data = ''' - - - - - - - - - - - - - - - - - - - - - - - - - ''' - expected = ''' + srdf_data = ''' + + + + + + + + + + + + + + + + + + + + + + + + + ''' + + expected = ''' @@ -142,84 +146,92 @@ def test_full_srdf(self): - ''' - robot = SRDF.from_xml_string(srdf_data) - self.assertTrue( xml_matches(robot.to_xml_string(),expected)) - + ''' + robot = SRDF.from_xml_string(srdf_data) + self.assertTrue( xml_matches(robot.to_xml_string(False),expected)) + + def test_simple_srdf(self): - datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/" - stream = open(datadir+'pr2_desc.1.srdf', 'r') - robot = SRDF.from_xml_string(stream.read()) - stream.close() - self.assertTrue(len(robot.virtual_joints)==0) - self.assertTrue(len(robot.groups)==0) - self.assertTrue(len(robot.group_states)==0) - self.assertTrue(len(robot.disable_collisionss)==0) - self.assertTrue(len(robot.end_effectors)==0) - - stream = open(datadir+'pr2_desc.2.srdf', 'r') - robot = SRDF.from_xml_string(stream.read()) - stream.close() - self.assertTrue(len(robot.virtual_joints)==1) - self.assertTrue(len(robot.groups)==1) - self.assertTrue(len(robot.group_states)==0) - self.assertTrue(len(robot.disable_collisionss)==0) - self.assertTrue(len(robot.end_effectors)==0) - + _, package_path = get_resource("packages", PKG) + fname = os.path.join( + package_path, "share", PKG, "resources", "pr2_desc.1.srdf") + stream = open(fname, 'r') + robot = SRDF.from_xml_string(stream.read()) + stream.close() + self.assertTrue(len(robot.virtual_joints)==0) + self.assertTrue(len(robot.groups)==0) + self.assertTrue(len(robot.group_states)==0) + self.assertTrue(len(robot.disable_collisionss)==0) + self.assertTrue(len(robot.end_effectors)==0) + + def test_medium_srdf(self): + _, package_path = get_resource("packages", PKG) + fname = os.path.join( + package_path, "share", PKG, "resources", "pr2_desc.2.srdf") + stream = open(fname, 'r') + robot = SRDF.from_xml_string(stream.read()) + stream.close() + self.assertTrue(len(robot.virtual_joints)==1) + self.assertTrue(len(robot.groups)==1) + self.assertTrue(len(robot.group_states)==0) + self.assertTrue(len(robot.disable_collisionss)==0) + self.assertTrue(len(robot.end_effectors)==0) + def test_complex_srdf(self): - datadir=rospkg.RosPack().get_path('srdfdom')+"/test/resources/" - stream = open(datadir+'pr2_desc.3.srdf', 'r') - robot = SRDF.from_xml_string(stream.read()) - stream.close() - self.assertTrue(len(robot.virtual_joints)==1) - self.assertTrue(len(robot.groups)==7) - self.assertTrue(len(robot.group_states)==2) - self.assertTrue(len(robot.disable_collisionss)==2) - self.assertTrue(robot.disable_collisionss[0].reason=="adjacent") - self.assertTrue(len(robot.end_effectors)==2) - - self.assertTrue(robot.virtual_joints[0].name=="world_joint") - self.assertTrue(robot.virtual_joints[0].type=="planar") - - for group in robot.groups: - if (group.name == "left_arm" or group.name == "right_arm" ): - self.assertTrue(len(group.chains)==1) - if group.name == "arms": - self.assertTrue(len(group.subgroups)==2) - if group.name == "base": - self.assertTrue(len(group.joints)==1) - if (group.name == "l_end_effector" or group.name == "r_end_effector" ): - self.assertTrue(len(group.links)==1) - self.assertTrue(len(group.joints)==9) - if group.name == "whole_body" : - self.assertTrue(len(group.joints)==1) - self.assertTrue(len(group.subgroups)==2) - - index=0 - if robot.group_states[0].group !="arms": - index=1 - - self.assertTrue(robot.group_states[index].group =="arms") - self.assertTrue(robot.group_states[index].name =="tuck_arms") - self.assertTrue(robot.group_states[1-index].group =="base") - self.assertTrue(robot.group_states[1-index].name =="home") - - v=next((joint.value for joint in robot.group_states[index].joints if joint.name=="l_shoulder_pan_joint"),None) - self.assertTrue(len(v) == 1) - self.assertTrue(v[0] ==0.2) - - w=next((joint.value for joint in robot.group_states[1-index].joints if joint.name=="world_joint"),None) - self.assertTrue(len(w) == 3) - self.assertTrue(w[0] ==0.4) - self.assertTrue(w[1] ==0) - self.assertTrue(w[2] ==-1) - - index = 0 if (robot.end_effectors[0].name[0] == 'r') else 1 - self.assertTrue(robot.end_effectors[index].name == 'r_end_effector') - self.assertTrue(robot.end_effectors[index].group == 'r_end_effector') - self.assertTrue(robot.end_effectors[index].parent_link == 'r_wrist_roll_link') - + _, package_path = get_resource("packages", PKG) + fname = os.path.join( + package_path, "share", PKG, "resources", "pr2_desc.3.srdf") + stream = open(fname, 'r') + robot = SRDF.from_xml_string(stream.read()) + stream.close() + self.assertTrue(len(robot.virtual_joints)==1) + self.assertTrue(len(robot.groups)==7) + self.assertTrue(len(robot.group_states)==2) + self.assertTrue(len(robot.disable_collisionss)==2) + self.assertTrue(robot.disable_collisionss[0].reason=="adjacent") + self.assertTrue(len(robot.end_effectors)==2) + + self.assertTrue(robot.virtual_joints[0].name=="world_joint") + self.assertTrue(robot.virtual_joints[0].type=="planar") + + for group in robot.groups: + if (group.name == "left_arm" or group.name == "right_arm" ): + self.assertTrue(len(group.chains)==1) + if group.name == "arms": + self.assertTrue(len(group.subgroups)==2) + if group.name == "base": + self.assertTrue(len(group.joints)==1) + if (group.name == "l_end_effector" or group.name == "r_end_effector" ): + self.assertTrue(len(group.links)==1) + self.assertTrue(len(group.joints)==9) + if group.name == "whole_body" : + self.assertTrue(len(group.joints)==1) + self.assertTrue(len(group.subgroups)==2) + + index=0 + if robot.group_states[0].group !="arms": + index=1 + + self.assertTrue(robot.group_states[index].group =="arms") + self.assertTrue(robot.group_states[index].name =="tuck_arms") + self.assertTrue(robot.group_states[1-index].group =="base") + self.assertTrue(robot.group_states[1-index].name =="home") + + v=next((joint.value for joint in robot.group_states[index].joints if joint.name=="l_shoulder_pan_joint"),None) + self.assertTrue(len(v) == 1) + self.assertTrue(v[0] ==0.2) + + w=next((joint.value for joint in robot.group_states[1-index].joints if joint.name=="world_joint"),None) + self.assertTrue(len(w) == 3) + self.assertTrue(w[0] ==0.4) + self.assertTrue(w[1] ==0) + self.assertTrue(w[2] ==-1) + + index = 0 if (robot.end_effectors[0].name[0] == 'r') else 1 + self.assertTrue(robot.end_effectors[index].name == 'r_end_effector') + self.assertTrue(robot.end_effectors[index].group == 'r_end_effector') + self.assertTrue(robot.end_effectors[index].parent_link == 'r_wrist_roll_link') + if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'srdf_python_parser_test', TestSRDFParser) + unittest.main() \ No newline at end of file diff --git a/test/test_parser.cpp b/test/test_parser.cpp index abe87cb..50b40d1 100644 --- a/test/test_parser.cpp +++ b/test/test_parser.cpp @@ -36,17 +36,15 @@ #include #include +#include + #include #include #include -#ifndef TEST_RESOURCE_LOCATION -#define TEST_RESOURCE_LOCATION "." -#endif - struct ScopedLocale { - ScopedLocale(const char* name = "C") + ScopedLocale(const char* name) { backup_ = setlocale(LC_ALL, nullptr); // store current locale setlocale(LC_ALL, name); @@ -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) { - 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); @@ -82,27 +80,34 @@ urdf::ModelInterfaceSharedPtr loadURDF(const std::string& filename) } } -TEST(TestCpp, testSimple) +void testSimple(const char* locale_name) { + 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, locale_name); + urdf::ModelInterfaceSharedPtr u = loadURDF(test_resource_location + "/pr2_desc.urdf", locale_name); 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); @@ -110,13 +115,31 @@ TEST(TestCpp, testSimple) EXPECT_TRUE(s.getEndEffectors().size() == 0); } -TEST(TestCpp, testComplex) +TEST(TestCpp, testSimpleC) +{ + testSimple("C"); +} + +TEST(TestCpp, testSimpleUTF) +{ + testSimple("nl_NL.UTF-8"); +} + +void testComplex(const char* locale_name) { 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, locale_name); + urdf::ModelInterfaceSharedPtr u = loadURDF(test_resource_location + "/pr2_desc.urdf", locale_name); 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); @@ -129,11 +152,17 @@ TEST(TestCpp, testComplex) 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); @@ -169,11 +198,19 @@ TEST(TestCpp, testComplex) EXPECT_TRUE(s.getEndEffectors()[index].parent_link_ == "r_wrist_roll_link"); } +TEST(TestCpp, testComplexC) +{ + testComplex("C"); +} + +TEST(TestCpp, testComplexUTF) +{ + testComplex("nl_NL.UTF-8"); +} + 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();