Add BehaviorTree#53 content

HansRobo · HansRobo · commit 59eaab8be155 · 2019-12-11T17:07:49.000+09:00
diff --git a/include/behaviortree_cpp_v3/bt_factory.h b/include/behaviortree_cpp_v3/bt_factory.h
@@ -173,6 +173,13 @@ class BehaviorTreeFactory
      */
     void registerFromPlugin(const std::string &file_path);
 
+    /**
+     * @brief registerFromROSPlugins finds all shared libraries that export ROS plugins for behaviortree_cpp, and calls registerFromPlugin for each library.
+     * @throws If not compiled with ROS support or if the library cannot load for any reason
+     *
+     */
+    void registerFromROSPlugins();
+
     /**
      * @brief instantiateTreeNode creates an instance of a previously registered TreeNode.
      *
diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp
@@ -14,6 +14,11 @@
 #include "behaviortree_cpp_v3/utils/shared_library.h"
 #include "behaviortree_cpp_v3/xml_parsing.h"
 
+#ifdef USING_ROS
+#include "filesystem/path.h"
+#include <ros/package.h>
+#endif
+
 namespace BT
 {
 BehaviorTreeFactory::BehaviorTreeFactory()
@@ -131,6 +136,66 @@ void BehaviorTreeFactory::registerFromPlugin(const std::string& file_path)
     }
 }
 
+#ifdef USING_ROS
+
+    #ifdef _WIN32
+const char os_pathsep(';');   // NOLINT
+#else
+const char os_pathsep(':');   // NOLINT
+#endif
+
+// This function is a copy from the one in class_loader_imp.hpp in ROS pluginlib
+// package, licensed under BSD.
+// https://github.com/ros/pluginlib
+std::vector<std::string> getCatkinLibraryPaths()
+{
+    std::vector<std::string> lib_paths;
+    const char* env = std::getenv("CMAKE_PREFIX_PATH");
+    if (env)
+    {
+        const std::string env_catkin_prefix_paths(env);
+        std::vector<BT::StringView> catkin_prefix_paths =
+            splitString(env_catkin_prefix_paths, os_pathsep);
+        for (BT::StringView catkin_prefix_path : catkin_prefix_paths)
+        {
+            filesystem::path path(catkin_prefix_path.to_string());
+            filesystem::path lib("lib");
+            lib_paths.push_back((path / lib).str());
+        }
+    }
+    return lib_paths;
+}
+
+void BehaviorTreeFactory::registerFromROSPlugins()
+{
+    std::vector<std::string> plugins;
+    ros::package::getPlugins("behaviortree_cpp", "bt_lib_plugin", plugins, true);
+    std::vector<std::string> catkin_lib_paths = getCatkinLibraryPaths();
+
+    for (const auto& plugin : plugins)
+    {
+        auto filename = filesystem::path(plugin + BT::SharedLibrary::suffix());
+        for (const auto& lib_path : catkin_lib_paths)
+        {
+            const auto full_path = filesystem::path(lib_path) / filename;
+            if (full_path.exists())
+            {
+                std::cout << "Registering ROS plugins from " << full_path.str() << std::endl;
+                registerFromPlugin(full_path.str());
+                break;
+            }
+        }
+    }
+}
+#else
+
+    void BehaviorTreeFactory::registerFromROSPlugins()
+    {
+        throw RuntimeError("Using attribute [ros_pkg] in <include>, but this library was compiled "
+                           "without ROS support. Recompile the BehaviorTree.CPP using catkin");
+    }
+#endif
+
 std::unique_ptr<TreeNode> BehaviorTreeFactory::instantiateTreeNode(
         const std::string& name,
         const std::string& ID,
