Merge pull request #166 from resibots/utheque

Utheque: URDF-theque (library of URDFs)

.github/workflows/ci_linux.yml

@@ -80,6 +80,7 @@ jobs:
           export CC=/usr/bin/clang && export CXX=/usr/bin/clang++
           fi
           cd ${{github.workspace}}/cmake/example && mkdir -p build && cd build && cmake -DDART_DIR=$DART_DIR .. && make -j4
+          cd ${{github.workspace}}/cmake/example_utheque && mkdir -p build && cd build && cmake .. && make -j4
       - name: Run Python
         run: |
           if [ "$BUILD_PYTHON" = "ON" ]; then

README.md

@@ -8,7 +8,8 @@ robot_dart [![Build Status](https://github.com/resibots/robot_dart/actions/workf
 ## Authors
 
 - Author/Maintainer: Konstantinos Chatzilygeroudis
-- Other contributors: Vaios Papaspyros
+- Active contributors: Jean-Baptiste Mouret
+- Other contributors: Antoine Cully, Vassilis Vassiliades, Vaios Papaspyros
 
 ### Using the code
 

cmake/UthequeConfig.cmake.in

@@ -0,0 +1,32 @@
+# - 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@")
+
+find_package(Boost REQUIRED filesystem)
+
+set(Utheque_INCLUDE_DIRS "@Utheque_INCLUDE_DIRS@")
+
+set(Utheque_LIBRARIES "Boost::filesystem")
+
+add_library(Utheque INTERFACE IMPORTED)
+set_target_properties(Utheque PROPERTIES
+  INTERFACE_LINK_LIBRARIES "${Utheque_LIBRARIES}"
+  INTERFACE_COMPILE_DEFINITIONS @Utheque_PREFIX@
+  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 "@utheque_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()

cmake/example_utheque/CMakeLists.txt

@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 3.10 FATAL_ERROR)
+project(utheque_example)
+
+find_package(Utheque REQUIRED)
+
+add_executable(utheque_example example_utheque.cpp)
+set_target_properties(utheque_example PROPERTIES CXX_STANDARD 11 CXX_STANDARD_REQUIRED YES)
+target_link_libraries(utheque_example PUBLIC Utheque)

cmake/example_utheque/example_utheque.cpp

@@ -0,0 +1,9 @@
+#include <iostream>
+#include <utheque/utheque.hpp>
+
+int main()
+{
+    auto p = utheque::path("talos/talos.urdf", true); // verbose mode
+    std::cout << "Path of the URDF: " << p << std::endl;
+    return 0;
+}

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,48 +1878,14 @@ 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)
     {
         ROBOT_DART_EXCEPTION_ASSERT(!filename.empty(), "Empty URDF filename");
 
         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, false, std::string(ROBOT_DART_PREFIX));
             // store the name for future use
             _model_filename = model_file;
             _packages = packages;
@@ -1937,7 +1905,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, false, std::string(ROBOT_DART_PREFIX));
                     loader.addPackageDirectory(
                         std::get<0>(packages[i]), package_path + "/" + package);
                 }
@@ -1962,7 +1930,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, false, std::string(ROBOT_DART_PREFIX));
                 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,98 @@
+#ifndef UTHEQUE_HPP_
+#define UTHEQUE_HPP_
+
+#include <boost/algorithm/string.hpp>
+#include <boost/filesystem.hpp> // will move to std::filesystem
+#include <exception>
+#include <string>
+
+namespace utheque {
+#ifndef UTHEQUE_PREFIX
+    static constexpr char* DEFAULT_PREFIX = const_cast<char*>("/usr/local");
+#else
+    static constexpr char* DEFAULT_PREFIX = const_cast<char*>(UTHEQUE_PREFIX);
+#endif
+    /// 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
+    /// The default prefix is got :
+    ///     - from the cmake config (which defines UTHEQUE_PREFIX)
+    ///     - otherwise from -DUTHEQUE_PREFIX="..."
+    ///     - if no UTHEQUE_PREFIX, then use /usr/local
+    /// @arg urdf or package name (e.g. talos/talos.urdf or talos_description)
+    /// @arg prefix (default to UTHEQUE_PREFIX)
+    /// @arg verbose print search paths
+    /// @return the full (absolute) path where to find the URDF (e.g. /usr/local/share/utheque/)
+    static std::string directory(const std::string& filename, bool verbose = false, const std::string& prefix = DEFAULT_PREFIX)
+    {
+        namespace fs = boost::filesystem;
+        fs::path model_file(boost::trim_copy(filename));
+        if (verbose)
+            std::cout << "utheque: searching for [" << model_file.string() << "]" << std::endl;
+
+        if (model_file.string()[0] == '/')
+            return "/";
+
+        if (verbose)
+            std::cout << "utheque: not an absolute path" << std::endl;
+
+        // search current directory
+        if (fs::exists(model_file))
+            return fs::current_path().string();
+
+        if (verbose)
+            std::cout << "utheque: not found in current path [" << fs::current_path().string() << "]" << std::endl;
+
+        // search <current_directory>/robots
+        if (fs::exists(fs::path("utheque") / model_file))
+            return (fs::current_path() / fs::path("utheque")).string();
+
+        if (verbose)
+            std::cout << "utheque: not found in current path/utheque [" << (fs::current_path() / fs::path("utheque")).string() << "]" << std::endl;
+
+        // search $UTHEQUE_PATH
+        const char* env = std::getenv("UTHEQUE_PATH");
+        if (env != nullptr) {
+            if (verbose)
+                std::cout << "Utheque: $UTHEQUE_PATH: [" << env << "]" << std::endl;
+            fs::path env_path(env);
+            if (fs::exists(env_path / model_file))
+                return env_path.string();
+        }
+        else if (verbose)
+            std::cout << "Utheque: no $UTHEQUE_PATH" << std::endl;
+
+        if (verbose)
+            std::cout << "utheque: not found in $UTHEQUE_PATH" << std::endl;
+
+        // search PREFIX/share/utheque
+        fs::path system_path(prefix + "/share/utheque");
+        if (fs::exists(system_path / model_file))
+            return system_path.string();
+
+        if (verbose)
+            std::cout << "utheque: not found in [" << system_path.string() << "]" << std::endl;
+
+        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, bool verbose = false, const std::string& prefix = DEFAULT_PREFIX)
+    {
+        namespace fs = boost::filesystem;
+        auto file_dir = fs::path(directory(filename, verbose, prefix));
+        auto model_file = file_dir / fs::path(boost::trim_copy(filename));
+        return model_file.string();
+    }
+} // namespace utheque
+
+#endif