Add simple test

rt-net · Nov 22, 2023 · f83743d · f83743d
1 parent 48f8b19
commit f83743d
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 9 deletions.
diff --git a/rt_manipulators_lib/CMakeLists.txt b/rt_manipulators_lib/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(ament_cmake REQUIRED)
 find_package(dynamixel_sdk REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3)
+find_package(urdfdom REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 
 set(library_name ${CMAKE_PROJECT_NAME})
@@ -41,6 +42,7 @@ target_include_directories(${library_name}
 ament_target_dependencies(${library_name}
     dynamixel_sdk
     Eigen3
+    urdfdom
     yaml_cpp_vendor
 )
 
@@ -49,6 +51,7 @@ ament_export_dependencies(
     dynamixel_sdk
     eigen3_cmake_module
     Eigen3
+    urdfdom
     yaml_cpp_vendor
 )
 install(
@@ -67,15 +70,17 @@ install(
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest)
+
+  find_package(ament_index_cpp REQUIRED)
   ament_add_gtest(test_kinematics_ros_utils
     ros_test/test_kinematics_ros_utils.cpp)
 
-  target_link_libraries(${test_kinematics_ros_utils}
+  target_link_libraries(test_kinematics_ros_utils
     ${library_name}
   )
-  target_include_directories(test_kinematics_ros_utils PRIVATE
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>)
+  ament_target_dependencies(test_kinematics_ros_utils
+    ament_index_cpp
+  )
 
   install(DIRECTORY
     ros_test/urdf

diff --git a/rt_manipulators_lib/package.xml b/rt_manipulators_lib/package.xml
@@ -14,9 +14,11 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <depend>ament_index_cpp</depend>
   <depend>dynamixel_sdk</depend>
   <depend>eigen</depend>
   <depend>eigen3_cmake_module</depend>
+  <depend>urdfdom</depend>
   <depend>yaml-cpp</depend>
   <depend>yaml_cpp_vendor</depend>
 

diff --git a/rt_manipulators_lib/ros_test/test_kinematics_ros_utils.cpp b/rt_manipulators_lib/ros_test/test_kinematics_ros_utils.cpp
@@ -12,23 +12,26 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <string>
-#include <vector>
+#include <ament_index_cpp/get_package_share_directory.hpp>
 
 #include "gtest/gtest.h"
-#include "rt_manipulators_cpp/link.hpp"
+#include "kinematics_ros_utils.hpp"
 
 class KinematicsROSUtilsFixture: public ::testing::Test {
  protected:
   virtual void SetUp() {
+    const auto path = ament_index_cpp::get_package_share_directory("rt_manipulators_cpp") + "/urdf/test_robot.urdf";
+    links = kinematics_ros_utils::parse_urdf_file(path);
   }
 
   virtual void TearDown() {
   }
 
-  std::vector<manipulators_link::Link> links;
+  kinematics_utils::links_t links;
 };
 
 TEST_F(KinematicsROSUtilsFixture, load_link_names) {
-  EXPECT_EQ("test_base", "test_base");
+  EXPECT_EQ(links[1].name, "base_link");
+  EXPECT_EQ(links[2].name, "r_leg_link1");
+  EXPECT_EQ(links[3].name, "r_leg_link2");
 }
diff --git a/rt_manipulators_lib/src/kinematics_ros_utils.cpp b/rt_manipulators_lib/src/kinematics_ros_utils.cpp
@@ -12,12 +12,31 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "urdf_parser/urdf_parser.h"
+
 #include "kinematics_ros_utils.hpp"
+#include <iostream>
 
 namespace kinematics_ros_utils {
 
 kinematics_utils::links_t parse_urdf_file(const std::string & path) {
   std::vector<manipulators_link::Link> links;
+  links.push_back(manipulators_link::Link());  // 0番目には空のリンクをセット
+
+  auto parsed_urdf = urdf::parseURDFFile(path);
+
+  manipulators_link::Link link;
+
+  // Set root_link
+  auto urdf_root_link = parsed_urdf->getRoot();
+  link.name = urdf_root_link->name;
+  links.push_back(link);
+
+  for (const auto & child_link : urdf_root->child_links) {
+    link.name = child_link->name;
+    links.push_back(link);
+  }
+
   return links;
 }