diff --git a/include/behaviortree_cpp_v3/bt_factory.h b/include/behaviortree_cpp_v3/bt_factory.h index dd92ccd15..8fc84d5f8 100644 --- 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 index 7170c2769..a51c74673 100644 --- 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 +#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 getCatkinLibraryPaths() +{ + std::vector lib_paths; + const char* env = std::getenv("CMAKE_PREFIX_PATH"); + if (env) + { + const std::string env_catkin_prefix_paths(env); + std::vector 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 plugins; + ros::package::getPlugins("behaviortree_cpp", "bt_lib_plugin", plugins, true); + std::vector 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 , but this library was compiled " + "without ROS support. Recompile the BehaviorTree.CPP using catkin"); + } +#endif + std::unique_ptr BehaviorTreeFactory::instantiateTreeNode( const std::string& name, const std::string& ID,