Utheque: URDF-theque (library of URDFs)

cmake/UthequeConfig.cmake.in

@@ -0,0 +1,30 @@
+# - Config file for the utheque package
+# example:
+# find_package(Utheque REQUIRED)
+# add_executable(your_example example.cpp)
+# target_link_libraries(your_example Utheque)
+
+include(CMakeFindDependencyMacro)
+include(FindPackageHandleStandardArgs)
+
+# CMAKE_MODULE_PATH
+set(CMAKE_MODULE_PATH "@Utheque_CMAKE_MODULE_PATH@")
+
+set(Utheque_INCLUDE_DIRS "@Utheque_INCLUDE_DIRS@")
+
+
+find_package(Boost REQUIRED filesystem)
+
+set(Utheque_LIBRARIES "Threads::Threads;Eigen3::Eigen;Boost::filesystem")
+
+set_target_properties(Utheque PROPERTIES
+  INTERFACE_INCLUDE_DIRECTORIES "${Utheque_INCLUDE_DIRS}")
+
+# Handle the QUIET and REQUIRED arguments
+find_package_handle_standard_args(
+    Utheque #Package name
+    DEFAULT_MSG
+    # Variables required to evaluate as TRUE
+    Utheque_INCLUDE_DIRS)
+
+mark_as_advanced(Utheque_INCLUDE_DIRS Utheque_FOUND)

cmake/UthequeConfigVersion.cmake.in

@@ -0,0 +1,11 @@
+set(PACKAGE_VERSION "@robot_dart_VERSION@")
+
+# Check whether the requested PACKAGE_FIND_VERSION is compatible
+if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
+  set(PACKAGE_VERSION_COMPATIBLE FALSE)
+else()
+  set(PACKAGE_VERSION_COMPATIBLE TRUE)
+  if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}")
+    set(PACKAGE_VERSION_EXACT TRUE)
+  endif()
+endif()

src/robot_dart/robot.cpp

@@ -10,6 +10,8 @@
 
 #include <robot_dart/control/robot_control.hpp>
 
+#include <utheque/utheque.hpp> // library of URDF
+
 namespace robot_dart {
     namespace detail {
         template <int content>
@@ -1876,38 +1878,7 @@ namespace robot_dart {
 
     const std::vector<std::pair<dart::dynamics::BodyNode*, double>>& Robot::drawing_axes() const { return _axis_shapes; }
 
-    std::string Robot::_get_path(const std::string& filename) const
-    {
-        namespace fs = boost::filesystem;
-        fs::path model_file(boost::trim_copy(filename));
-        if (model_file.string()[0] == '/')
-            return "/";
-
-        // search current directory
-        if (fs::exists(model_file))
-            return fs::current_path().string();
-
-        // search <current_directory>/robots
-        if (fs::exists(fs::path("robots") / model_file))
-            return (fs::current_path() / fs::path("robots")).string();
-
-        // search $ROBOT_DART_PATH
-        const char* env = std::getenv("ROBOT_DART_PATH");
-        if (env != nullptr) {
-            fs::path env_path(env);
-            if (fs::exists(env_path / model_file))
-                return env_path.string();
-        }
-
-        // search PREFIX/share/robot_dart/robots
-        fs::path system_path(std::string(ROBOT_DART_PREFIX) + "/share/robot_dart/robots/");
-        if (fs::exists(system_path / model_file))
-            return system_path.string();
-
-        ROBOT_DART_EXCEPTION_ASSERT(false, std::string("Could not find :") + filename);
-
-        return std::string();
-    }
+
 
     dart::dynamics::SkeletonPtr Robot::_load_model(const std::string& filename, const std::vector<std::pair<std::string, std::string>>& packages, bool is_urdf_string)
     {
@@ -1916,8 +1887,7 @@ namespace robot_dart {
         dart::dynamics::SkeletonPtr tmp_skel;
         if (!is_urdf_string) {
             // search for the right directory for our files
-            std::string file_dir = _get_path(filename);
-            std::string model_file = file_dir + '/' + boost::trim_copy(filename);
+            std::string model_file = utheque::path(filename);
             // store the name for future use
             _model_filename = model_file;
             _packages = packages;
@@ -1937,7 +1907,7 @@ namespace robot_dart {
 #endif
                 for (size_t i = 0; i < packages.size(); i++) {
                     std::string package = std::get<1>(packages[i]);
-                    std::string package_path = _get_path(package);
+                    std::string package_path = utheque::directory(package);
                     loader.addPackageDirectory(
                         std::get<0>(packages[i]), package_path + "/" + package);
                 }
@@ -1962,7 +1932,7 @@ namespace robot_dart {
             dart::io::DartLoader loader;
             for (size_t i = 0; i < packages.size(); i++) {
                 std::string package = std::get<1>(packages[i]);
-                std::string package_path = _get_path(package);
+                std::string package_path = utheque::directory(package);
                 loader.addPackageDirectory(std::get<0>(packages[i]), package_path + "/" + package);
             }
             tmp_skel = loader.parseSkeletonString(filename, "");

src/tests/test_control.cpp

@@ -40,7 +40,7 @@ BOOST_AUTO_TEST_CASE(test_pd_control)
     BOOST_CHECK(!pd_control->active());
 
     // load a robot
-    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/robots/pendulum.urdf");
+    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/utheque/pendulum.urdf");
     BOOST_REQUIRE(pendulum);
 
     // set proper parameters
@@ -95,7 +95,7 @@ BOOST_AUTO_TEST_CASE(test_simple_control)
     BOOST_CHECK(!simple_control->active());
 
     // load a robot
-    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/robots/pendulum.urdf");
+    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/utheque/pendulum.urdf");
     BOOST_REQUIRE(pendulum);
     pendulum->fix_to_world();
 
@@ -125,7 +125,7 @@ BOOST_AUTO_TEST_CASE(test_simple_control)
 BOOST_AUTO_TEST_CASE(test_robot_control)
 {
     // load a robot
-    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/robots/pendulum.urdf");
+    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/utheque/pendulum.urdf");
     BOOST_REQUIRE(pendulum);
     pendulum->fix_to_world();
 

src/tests/test_robot.cpp

@@ -28,7 +28,7 @@ BOOST_AUTO_TEST_CASE(test_constructors)
     BOOST_CHECK(robot == nullptr);
 
     // well-defined URDF
-    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/robots/pendulum.urdf");
+    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/utheque/pendulum.urdf");
     BOOST_REQUIRE(pendulum);
     // well-defined skeleton
     dart::dynamics::SkeletonPtr dummy_skel = dart::dynamics::Skeleton::create("dummy");
@@ -46,7 +46,7 @@ BOOST_AUTO_TEST_CASE(test_constructors)
 
 BOOST_AUTO_TEST_CASE(test_dof_maps)
 {
-    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/robots/pendulum.urdf");
+    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/utheque/pendulum.urdf");
     BOOST_REQUIRE(pendulum);
 
     // check dofs
@@ -62,7 +62,7 @@ BOOST_AUTO_TEST_CASE(test_dof_maps)
 
 BOOST_AUTO_TEST_CASE(test_fix_free)
 {
-    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/robots/pendulum.urdf");
+    auto pendulum = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/utheque/pendulum.urdf");
     BOOST_REQUIRE(pendulum);
 
     pendulum->fix_to_world();
@@ -80,7 +80,7 @@ BOOST_AUTO_TEST_CASE(test_fix_free)
 
 BOOST_AUTO_TEST_CASE(test_actuators_and_dofs)
 {
-    auto pexod = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/robots/pexod.urdf");
+    auto pexod = std::make_shared<Robot>(std::string(ROBOT_DART_BUILD_DIR) + "/utheque/pexod.urdf");
     BOOST_REQUIRE(pexod);
     // fix to world
     pexod->fix_to_world();

src/utheque/utheque.hpp

@@ -0,0 +1,66 @@
+#ifndef UTHEQUE_HPP_
+#define UTHEQUE_HPP_
+
+#include <string>
+#include <exception>
+#include <boost/algorithm/string.hpp>
+#include <boost/filesystem.hpp> // will move to std::filesystem
+
+namespace utheque {
+    static constexpr char* UTHEQUE_DEFAULT_PREFIX = "/usr/local";
+    /// return the directory  where to find the urdf (or urdf package) from the utheque (URDF library)
+    /// if start by /, do nothing
+    /// otherwise, search (in this order): 
+    ///    - in the current directory
+    ///    - in <current_directory>/utheque/ 
+    ///    - $UTHEQUE_PATH
+    ///    - in <prefix>/share/utheque
+    /// @arg urdf or package name (e.g. talos/talos.urdf or talos_description)
+    /// @arg prefix /usr/local/
+    /// @return the full (absolute) path where to find the URDF (e.g. /usr/local/share/utheque/)
+    static std::string directory(const std::string& filename, const std::string prefix = UTHEQUE_DEFAULT_PREFIX)
+    {
+        namespace fs = boost::filesystem;
+        fs::path model_file(boost::trim_copy(filename));
+        if (model_file.string()[0] == '/')
+            return "/";
+
+        // search current directory
+        if (fs::exists(model_file))
+            return fs::current_path().string();
+
+        // search <current_directory>/robots
+        if (fs::exists(fs::path("utheque") / model_file))
+            return (fs::current_path() / fs::path("utheque")).string();
+
+        // search $UTHEQUE_PATH
+        const char* env = std::getenv("UTHEQUE_PATH");
+        if (env != nullptr) {
+            fs::path env_path(env);
+            if (fs::exists(env_path / model_file))
+                return env_path.string();
+        }
+
+        // search PREFIX/share/utheque
+        fs::path system_path(prefix + "/share/utheque");
+        if (fs::exists(system_path / model_file))
+            return system_path.string();
+
+        throw std::runtime_error(std::string("Utheque::could not find :") + filename);
+
+        return std::string();
+    }
+
+    /// call directory() and put the filename back in the path
+    /// @arg urdf or package name (e.g. talos/talos.urdf or talos_description)
+    /// @arg prefix /usr/local/
+    /// @return full path of the URDF file: (e.g. /usr/local/share/utheque/talos/talos.urdf)
+    static std::string path(const std::string& filename, const std::string& prefix = UTHEQUE_DEFAULT_PREFIX) {
+        namespace fs = boost::filesystem;
+        auto file_dir = fs::path(directory(filename, prefix));
+        auto model_file = file_dir / fs::path(boost::trim_copy(filename));
+        return model_file.string();
+    }
+}
+
+#endif