niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Introduce rclcpp_components to implement composition (#665)

Browse source 
* Introduce rclcpp_components package

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Keep pointer to NodeWrapper vs NodeInterface.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Remove component registration from rclcpp

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Make topics names private-prefix.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Handle name and namespace with remap rules.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Linting.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Address reviewer feedback.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Change to smart pointers for managing memory.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Update to use rcpputils filesystem/split.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Address reviewer feedback and add docs.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Add tests around ComponentManager.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Lint.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Address reviewer feedback and add overflow check.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Fix CI.

Signed-off-by: Michael Carroll <michael@openrobotics.org>
This commit is contained in:
Michael Carroll 2019-04-04 11:16:32 -05:00 • committed by GitHub
parent d11a10a583
commit 0f25f714fe
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
18 changed files with 1243 additions and 56 deletions
Download patch file Download diff file Expand all files Collapse all files

122
rclcpp_components/CMakeLists.txt Normal file
View file

@ -0,0 +1,122 @@
cmake_minimum_required(VERSION 3.5)
project(rclcpp_components)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake_ros REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(class_loader REQUIRED)
find_package(composition_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rcpputils REQUIRED)
include_directories(include)
add_library(
component_manager
STATIC
src/component_manager.cpp
)
ament_target_dependencies(component_manager
"ament_index_cpp"
"class_loader"
"composition_interfaces"
"rclcpp"
"rcpputils"
)
add_executable(
component_container
src/component_container.cpp
)
target_link_libraries(component_container component_manager)
ament_target_dependencies(component_container
"rclcpp"
)
add_executable(
component_container_mt
src/component_container_mt.cpp
)
target_link_libraries(component_container_mt component_manager)
ament_target_dependencies(component_container_mt
"rclcpp"
)
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
target_link_libraries(component_container "stdc++fs")
target_link_libraries(component_container_mt "stdc++fs")
endif()
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
set(components "")
add_library(test_component SHARED test/components/test_component.cpp)
ament_target_dependencies(test_component
"class_loader"
"rclcpp"
"rclcpp_components")
#rclcpp_components_register_nodes(test_component "test_rclcpp_components::TestComponent")
set(components "${components}test_rclcpp_components::TestComponentFoo;$<TARGET_FILE:test_component>\n")
set(components "${components}test_rclcpp_components::TestComponentBar;$<TARGET_FILE:test_component>\n")
set(components "${components}test_rclcpp_components::TestComponentNoNode;$<TARGET_FILE:test_component>\n")
file(GENERATE
OUTPUT
"${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$<CONFIG>/share/ament_index/resource_index/rclcpp_components/${PROJECT_NAME}"
CONTENT "${components}")
set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}")
if(WIN32)
set(append_library_dirs "${append_library_dirs}/$<CONFIG>")
endif()
ament_add_gtest(test_component_manager test/test_component_manager.cpp
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$<CONFIG>
APPEND_LIBRARY_DIRS "${append_library_dirs}")
if(TARGET test_component_manager)
target_link_libraries(test_component_manager component_manager)
target_include_directories(test_component_manager PRIVATE src)
endif()
ament_add_gtest(test_component_manager_api test/test_component_manager_api.cpp
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$<CONFIG>
APPEND_LIBRARY_DIRS "${append_library_dirs}")
if(TARGET test_component_manager_api)
target_link_libraries(test_component_manager_api component_manager)
target_include_directories(test_component_manager_api PRIVATE src)
endif()
endif()
# Install executables
install(
TARGETS component_container component_container_mt
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
# Install include directories
install(
DIRECTORY include/
DESTINATION include
)
# Install cmake
install(
DIRECTORY cmake
DESTINATION share/${PROJECT_NAME}
)
# specific order: dependents before dependencies
ament_export_include_directories(include)
ament_export_dependencies(class_loader)
ament_export_dependencies(rclcpp)
ament_package(CONFIG_EXTRAS rclcpp_components-extras.cmake)

18
rclcpp_components/cmake/rclcpp_components_package_hook.cmake Normal file
View file

@ -0,0 +1,18 @@
# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# register node plugins
ament_index_register_resource(
"rclcpp_components" CONTENT "${_RCLCPP_COMPONENTS__NODES}")

62
rclcpp_components/cmake/rclcpp_components_register_nodes.cmake Normal file
View file

@ -0,0 +1,62 @@
# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Register an rclcpp component with the ament resource index.
#
# The passed library can contain multiple nodes each registered via macro.
#
# :param target: the shared library target
# :type target: string
# :param ARGN: the unique plugin names being exported using class_loader
# :type ARGN: list of strings
#
macro(rclcpp_components_register_nodes target)
if(NOT TARGET ${target})
message(
FATAL_ERROR
"rclcpp_components_register_nodes() first argument "
"'${target}' is not a target")
endif()
get_target_property(_target_type ${target} TYPE)
if(NOT _target_type STREQUAL "SHARED_LIBRARY")
message(
FATAL_ERROR
"rclcpp_components_register_nodes() first argument "
"'${target}' is not a shared library target")
endif()
if(${ARGC} GREATER 0)
_rclcpp_components_register_package_hook()
set(_unique_names)
foreach(_arg ${ARGN})
if(_arg IN_LIST _unique_names)
message(
FATAL_ERROR
"rclcpp_components_register_nodes() the plugin names "
"must be unique (multiple '${_arg}')")
endif()
list(APPEND _unique_names "${_arg}")
if(WIN32)
set(_path "bin")
else()
set(_path "lib")
endif()
set(_RCLCPP_COMPONENTS__NODES
"${_RCLCPP_COMPONENTS__NODES}${_arg};${_path}/$<TARGET_FILE_NAME:${target}>\n")
endforeach()
endif()
endmacro()

46
rclcpp_components/include/rclcpp_components/node_factory.hpp Normal file
View file

@ -0,0 +1,46 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_COMPONENTS__NODE_FACTORY_HPP__
#define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__
#include "rclcpp_components/node_instance_wrapper.hpp"
namespace rclcpp_components
{
/// The NodeFactory interface is used by the class loader to instantiate components.
/**
* The NodeFactory interface serves two purposes:
* * It allows for classes not derived from `rclcpp::Node` to be used as components.
* * It allows derived constructors to be called when components are loaded.
*/
class NodeFactory
{
public:
NodeFactory() = default;
virtual ~NodeFactory() = default;
/// Create an instance of a component
/**
* \param[in] options Additional options used in the construction of the component.
*/
virtual
NodeInstanceWrapper
create_node_instance(const rclcpp::NodeOptions & options) = 0;
};
} // namespace rclcpp_components
#endif // RCLCPP_COMPONENTS__NODE_FACTORY_HPP__

53
rclcpp_components/include/rclcpp_components/node_factory_template.hpp Normal file
View file

@ -0,0 +1,53 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__
#define RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__
#include <functional>
#include <memory>
#include "rclcpp_components/node_factory.hpp"
namespace rclcpp_components
{
/// NodeFactoryTemplate is a convenience class for instantiating components.
/**
* The NodeFactoryTemplate class can be used to provide the NodeFactory interface for
* components that implement a single-argument constructor and `get_node_base_interface`.
*/
template<typename NodeT>
class NodeFactoryTemplate : public NodeFactory
{
public:
NodeFactoryTemplate() = default;
virtual ~NodeFactoryTemplate() = default;
/// Create an instance of a component
/**
* \param[in] options Additional options used in the construction of the component.
*/
NodeInstanceWrapper
create_node_instance(const rclcpp::NodeOptions & options) override
{
auto node = std::make_shared<NodeT>(options);
return NodeInstanceWrapper(
node, std::bind(&NodeT::get_node_base_interface, node));
}
};
} // namespace rclcpp_components
#endif // RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__

71
rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp Normal file
View file

@ -0,0 +1,71 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__
#define RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__
#include <functional>
#include <memory>
#include "rclcpp/node_interfaces/node_base_interface.hpp"
namespace rclcpp_components
{
/// The NodeInstanceWrapper encapsulates the node instance.
class NodeInstanceWrapper
{
public:
using NodeBaseInterfaceGetter = std::function<
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr(const std::shared_ptr<void> &)>;
NodeInstanceWrapper()
: node_instance_(nullptr)
{}
NodeInstanceWrapper(
std::shared_ptr<void> node_instance,
NodeBaseInterfaceGetter node_base_interface_getter)
: node_instance_(node_instance), node_base_interface_getter_(node_base_interface_getter)
{}
/// Get a type-erased pointer to the original Node instance
/**
* This is only for debugging and special cases.
* For most cases `get_node_base_interface` will be sufficient.
*
* \return Shared pointer to the encapsulated Node instance.
*/
const std::shared_ptr<void>
get_node_instance() const
{
return node_instance_;
}
/// Get NodeBaseInterface pointer for the encapsulated Node Instance.
/**
* \return Shared NodeBaseInterface pointer of the encapsulated Node instance.
*/
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
get_node_base_interface()
{
return node_base_interface_getter_(node_instance_);
}
private:
std::shared_ptr<void> node_instance_;
NodeBaseInterfaceGetter node_base_interface_getter_;
};
} // namespace rclcpp_components
#endif // RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__

37
rclcpp_components/include/rclcpp_components/register_node_macro.hpp Normal file
View file

@ -0,0 +1,37 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__
#define RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__
#include "class_loader/class_loader.hpp"
#include "rclcpp_components/node_factory_template.hpp"
/// Register a component that can be dynamically loaded at runtime.
/**
* The registration macro should appear once per component per library.
* The macro should appear in a single translation unit.
*
* Valid arguments for NodeClass shall:
* * Have a constructor that takes a single argument that is a `rclcpp::NodeOptions` instance.
* * Have a method of of the signature:
* `rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface`
*
* Note: NodeClass does not need to inherit from `rclcpp::Node`, but it is the easiest way.
*/
#define RCLCPP_COMPONENTS_REGISTER_NODE(NodeClass) \
CLASS_LOADER_REGISTER_CLASS(rclcpp_components::NodeFactoryTemplate<NodeClass>, \
rclcpp_components::NodeFactory)
#endif // RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__

33
rclcpp_components/package.xml Normal file
View file

@ -0,0 +1,33 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>rclcpp_components</name>
<version>0.6.2</version>
<description>Package containing tools for dynamically loadable components</description>
<maintainer email="michael@openrobotics.org">Michael Carroll</maintainer>
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake_ros</buildtool_depend>
<build_depend>ament_index_cpp</build_depend>
<build_depend>class_loader</build_depend>
<build_depend>composition_interfaces</build_depend>
<build_depend>rclcpp</build_depend>
<build_depend>rcpputils</build_depend>
<exec_depend>ament_index_cpp</exec_depend>
<exec_depend>class_loader</exec_depend>
<exec_depend>composition_interfaces</exec_depend>
<exec_depend>rclcpp</exec_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>launch_testing</test_depend>
<test_depend>std_msgs</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

29
rclcpp_components/rclcpp_components-extras.cmake Normal file
View file

@ -0,0 +1,29 @@
# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# copied from rclcpp_components/rclcpp_components-extras.cmake
# register ament_package() hook for node plugins once.
macro(_rclcpp_components_register_package_hook)
if(NOT DEFINED _RCLCPP_COMPONENTS_PACKAGE_HOOK_REGISTERED)
set(_RCLCPP_COMPONENTS_PACKAGE_HOOK_REGISTERED TRUE)
find_package(ament_cmake_core QUIET REQUIRED)
ament_register_extension("ament_package" "rclcpp_components"
"rclcpp_components_package_hook.cmake")
endif()
endmacro()
include("${rclcpp_components_DIR}/rclcpp_components_register_nodes.cmake")

29
rclcpp_components/src/component_container.cpp Normal file
View file

@ -0,0 +1,29 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "component_manager.hpp"
int main(int argc, char * argv[])
{
/// Component container with a single-threaded executor.
rclcpp::init(argc, argv);
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
auto node = std::make_shared<rclcpp_components::ComponentManager>(exec);
exec->add_node(node);
exec->spin();
}

29
rclcpp_components/src/component_container_mt.cpp Normal file
View file

@ -0,0 +1,29 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "component_manager.hpp"
int main(int argc, char * argv[])
{
/// Component container with a multi-threaded executor.
rclcpp::init(argc, argv);
auto exec = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
auto node = std::make_shared<rclcpp_components::ComponentManager>(exec);
exec->add_node(node);
exec->spin();
}

239
rclcpp_components/src/component_manager.cpp Normal file
View file

@ -0,0 +1,239 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "component_manager.hpp"
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include "ament_index_cpp/get_resource.hpp"
#include "rcpputils/filesystem_helper.hpp"
#include "rcpputils/split.hpp"
using namespace std::placeholders;
namespace rclcpp_components
{
ComponentManager::ComponentManager(
std::weak_ptr<rclcpp::executor::Executor> executor)
: Node("ComponentManager"),
executor_(executor)
{
loadNode_srv_ = create_service<LoadNode>("~/_container/load_node",
std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3));
unloadNode_srv_ = create_service<UnloadNode>("~/_container/unload_node",
std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3));
listNodes_srv_ = create_service<ListNodes>("~/_container/list_nodes",
std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3));
}
ComponentManager::~ComponentManager()
{
if (node_wrappers_.size()) {
RCLCPP_DEBUG(get_logger(), "Removing components from executor");
if (auto exec = executor_.lock()) {
for (auto & wrapper : node_wrappers_) {
exec->remove_node(wrapper.second.get_node_base_interface());
}
}
}
}
std::vector<ComponentManager::ComponentResource>
ComponentManager::get_component_resources(const std::string & package_name) const
{
std::string content;
std::string base_path;
if (!ament_index_cpp::get_resource(
"rclcpp_components", package_name, content, &base_path))
{
throw ComponentManagerException("Could not find requested resource in ament index");
}
std::vector<ComponentResource> resources;
std::vector<std::string> lines = rcpputils::split(content, '\n', true);
for (const auto & line : lines) {
std::vector<std::string> parts = rcpputils::split(line, ';');
if (parts.size() != 2) {
throw ComponentManagerException("Invalid resource entry");
}
std::string library_path = parts[1];
if (!rcpputils::fs::path(library_path).is_absolute()) {
library_path = base_path + "/" + library_path;
}
resources.push_back({parts[0], library_path});
}
return resources;
}
std::shared_ptr<rclcpp_components::NodeFactory>
ComponentManager::create_component_factory(const ComponentResource & resource)
{
std::string library_path = resource.second;
std::string class_name = resource.first;
std::string fq_class_name = "rclcpp_components::NodeFactoryTemplate<" + class_name + ">";
class_loader::ClassLoader * loader;
if (loaders_.find(library_path) == loaders_.end()) {
RCLCPP_INFO(get_logger(), "Load Library: %s", library_path.c_str());
try {
loaders_[library_path] = std::make_unique<class_loader::ClassLoader>(library_path);
} catch (const std::exception & ex) {
throw ComponentManagerException("Failed to load library: " + std::string(ex.what()));
} catch (...) {
throw ComponentManagerException("Failed to load library");
}
}
loader = loaders_[library_path].get();
auto classes = loader->getAvailableClasses<rclcpp_components::NodeFactory>();
for (const auto & clazz : classes) {
RCLCPP_INFO(get_logger(), "Found class: %s", clazz.c_str());
if (clazz == class_name || clazz == fq_class_name) {
RCLCPP_INFO(get_logger(), "Instantiate class: %s", clazz.c_str());
return loader->createInstance<rclcpp_components::NodeFactory>(clazz);
}
}
return {};
}
void
ComponentManager::OnLoadNode(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<LoadNode::Request> request,
std::shared_ptr<LoadNode::Response> response)
{
(void) request_header;
try {
auto resources = get_component_resources(request->package_name);
for (const auto & resource : resources) {
if (resource.first != request->plugin_name) {
continue;
}
auto factory = create_component_factory(resource);
if (factory == nullptr) {
continue;
}
std::vector<rclcpp::Parameter> parameters;
for (const auto & p : request->parameters) {
parameters.push_back(rclcpp::Parameter::from_parameter_msg(p));
}
std::vector<std::string> remap_rules {request->remap_rules};
if (!request->node_name.empty()) {
remap_rules.push_back("__node:=" + request->node_name);
}
if (!request->node_namespace.empty()) {
remap_rules.push_back("__ns:=" + request->node_namespace);
}
auto options = rclcpp::NodeOptions()
.initial_parameters(parameters)
.arguments(remap_rules);
auto node_id = unique_id++;
if (0 == node_id) {
// This puts a technical limit on the number of times you can add a component.
// But even if you could add (and remove) them at 1 kHz (very optimistic rate)
// it would still be a very long time before you could exhaust the pool of id's:
// 2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years
// So around 585 million years. Even at 1 GHz, it would take 585 years.
// I think it's safe to avoid trying to handle overflow.
// If we roll over then it's most likely a bug.
throw std::overflow_error("exhausted the unique ids for components in this process");
}
try {
node_wrappers_[node_id] = factory->create_node_instance(options);
} catch (...) {
// In the case that the component constructor throws an exception,
// rethrow into the following catch block.
throw ComponentManagerException("Component constructor threw an exception");
}
auto node = node_wrappers_[node_id].get_node_base_interface();
if (auto exec = executor_.lock()) {
exec->add_node(node, true);
}
response->full_node_name = node->get_fully_qualified_name();
response->unique_id = node_id;
response->success = true;
return;
}
RCLCPP_ERROR(
get_logger(), "Failed to find class with the requested plugin name '%s' in "
"the loaded library",
request->plugin_name.c_str());
response->error_message = "Failed to find class with the requested plugin name.";
response->success = false;
} catch (const ComponentManagerException & ex) {
RCLCPP_ERROR(get_logger(), ex.what());
response->error_message = ex.what();
response->success = false;
}
}
void
ComponentManager::OnUnloadNode(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<UnloadNode::Request> request,
std::shared_ptr<UnloadNode::Response> response)
{
(void) request_header;
auto wrapper = node_wrappers_.find(request->unique_id);
if (wrapper == node_wrappers_.end()) {
response->success = false;
std::stringstream ss;
ss << "No node found with unique_id: " << request->unique_id;
response->error_message = ss.str();
RCLCPP_WARN(get_logger(), ss.str());
} else {
if (auto exec = executor_.lock()) {
exec->remove_node(wrapper->second.get_node_base_interface());
}
node_wrappers_.erase(wrapper);
response->success = true;
}
}
void
ComponentManager::OnListNodes(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<ListNodes::Request> request,
std::shared_ptr<ListNodes::Response> response)
{
(void) request_header;
(void) request;
for (auto & wrapper : node_wrappers_) {
response->unique_ids.push_back(wrapper.first);
response->full_node_names.push_back(
wrapper.second.get_node_base_interface()->get_fully_qualified_name());
}
}
} // namespace rclcpp_components

104
rclcpp_components/src/component_manager.hpp Normal file
View file

@ -0,0 +1,104 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef COMPONENT_MANAGER_HPP__
#define COMPONENT_MANAGER_HPP__
#include <map>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "class_loader/class_loader.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/node_options.hpp"
#include "rclcpp/rclcpp.hpp"
#include "composition_interfaces/srv/load_node.hpp"
#include "composition_interfaces/srv/unload_node.hpp"
#include "composition_interfaces/srv/list_nodes.hpp"
#include "rclcpp_components/node_factory.hpp"
namespace rclcpp_components
{
class ComponentManagerException : public std::runtime_error
{
public:
explicit ComponentManagerException(const std::string & error_desc)
: std::runtime_error(error_desc) {}
};
class ComponentManager : public rclcpp::Node
{
public:
using LoadNode = composition_interfaces::srv::LoadNode;
using UnloadNode = composition_interfaces::srv::UnloadNode;
using ListNodes = composition_interfaces::srv::ListNodes;
/// Represents a component resource.
/**
* Is a pair of class name (for class loader) and library path (absolute)
*/
using ComponentResource = std::pair<std::string, std::string>;
ComponentManager(
std::weak_ptr<rclcpp::executor::Executor> executor);
~ComponentManager();
/// Return a list of valid loadable components in a given package.
std::vector<ComponentResource>
get_component_resources(const std::string & package_name) const;
std::shared_ptr<rclcpp_components::NodeFactory>
create_component_factory(const ComponentResource & resource);
private:
void
OnLoadNode(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<LoadNode::Request> request,
std::shared_ptr<LoadNode::Response> response);
void
OnUnloadNode(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<UnloadNode::Request> request,
std::shared_ptr<UnloadNode::Response> response);
void
OnListNodes(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<ListNodes::Request> request,
std::shared_ptr<ListNodes::Response> response);
private:
std::weak_ptr<rclcpp::executor::Executor> executor_;
uint64_t unique_id {1};
std::map<std::string, std::unique_ptr<class_loader::ClassLoader>> loaders_;
std::map<uint64_t, rclcpp_components::NodeInstanceWrapper> node_wrappers_;
rclcpp::Service<LoadNode>::SharedPtr loadNode_srv_;
rclcpp::Service<UnloadNode>::SharedPtr unloadNode_srv_;
rclcpp::Service<ListNodes>::SharedPtr listNodes_srv_;
};
} // namespace rclcpp_components
#endif // COMPONENT_MANAGER_HPP__

68
rclcpp_components/test/components/test_component.cpp Normal file
View file

@ -0,0 +1,68 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "rclcpp/rclcpp.hpp"
namespace test_rclcpp_components
{
/// Simple test component
class TestComponentFoo : public rclcpp::Node
{
public:
explicit TestComponentFoo(rclcpp::NodeOptions options)
: rclcpp::Node("test_component_foo", options)
{
}
};
/// Simple test component
class TestComponentBar : public rclcpp::Node
{
public:
explicit TestComponentBar(rclcpp::NodeOptions options)
: rclcpp::Node("test_component_bar", options)
{
}
};
/// Simple test component that doesn't inherit from rclcpp::Node
class TestComponentNoNode
{
public:
explicit TestComponentNoNode(rclcpp::NodeOptions options)
: node_("test_component_no_node", options)
{
}
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
get_node_base_interface()
{
return node_.get_node_base_interface();
}
private:
rclcpp::Node node_;
};
} // namespace test_rclcpp_components
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentFoo)
RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentBar)
RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentNoNode)

89
rclcpp_components/test/test_component_manager.cpp Normal file
View file

@ -0,0 +1,89 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include "component_manager.hpp"
#include "rcpputils/filesystem_helper.hpp"
class TestComponentManager : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
};
TEST_F(TestComponentManager, get_component_resources_invalid)
{
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
EXPECT_THROW(manager->get_component_resources("invalid_package"),
rclcpp_components::ComponentManagerException);
}
TEST_F(TestComponentManager, get_component_resources_valid)
{
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
auto resources = manager->get_component_resources("rclcpp_components");
EXPECT_EQ(3u, resources.size());
EXPECT_EQ("test_rclcpp_components::TestComponentFoo", resources[0].first);
EXPECT_EQ("test_rclcpp_components::TestComponentBar", resources[1].first);
EXPECT_EQ("test_rclcpp_components::TestComponentNoNode", resources[2].first);
EXPECT_TRUE(rcpputils::fs::path(resources[0].second).exists());
EXPECT_TRUE(rcpputils::fs::path(resources[1].second).exists());
EXPECT_TRUE(rcpputils::fs::path(resources[2].second).exists());
}
TEST_F(TestComponentManager, create_component_factory_valid)
{
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
auto resources = manager->get_component_resources("rclcpp_components");
EXPECT_EQ(3u, resources.size());
// Repeated loading should reuse existing class loader and not throw.
EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]););
EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]););
for (const auto & resource : resources) {
auto factory = manager->create_component_factory(resource);
EXPECT_NE(nullptr, factory);
}
}
TEST_F(TestComponentManager, create_component_factory_invalid)
{
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
// Test invalid library
EXPECT_THROW(manager->create_component_factory({"foo_class", "invalid_library.so"}),
rclcpp_components::ComponentManagerException);
// Test valid library with invalid class
auto resources = manager->get_component_resources("rclcpp_components");
auto factory = manager->create_component_factory({"foo_class", resources[0].second});
EXPECT_EQ(nullptr, factory);
}

298
rclcpp_components/test/test_component_manager_api.cpp Normal file
View file

@ -0,0 +1,298 @@
// Copyright 2019 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include "composition_interfaces/srv/load_node.hpp"
#include "composition_interfaces/srv/unload_node.hpp"
#include "composition_interfaces/srv/list_nodes.hpp"
#include "component_manager.hpp"
using namespace std::chrono_literals;
class TestComponentManager : public ::testing::Test
{
protected:
static void SetUpTestCase()
{
rclcpp::init(0, nullptr);
}
};
TEST_F(TestComponentManager, load_components)
{
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
auto node = rclcpp::Node::make_shared("test_component_manager");
auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
exec->add_node(manager);
exec->add_node(node);
auto client = node->create_client<composition_interfaces::srv::LoadNode>(
"/ComponentManager/_container/load_node");
if (!client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}
{
auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
request->package_name = "rclcpp_components";
request->plugin_name = "test_rclcpp_components::TestComponentFoo";
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, true);
EXPECT_EQ(result.get()->error_message, "");
EXPECT_EQ(result.get()->full_node_name, "/test_component_foo");
EXPECT_EQ(result.get()->unique_id, 1u);
}
{
auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
request->package_name = "rclcpp_components";
request->plugin_name = "test_rclcpp_components::TestComponentBar";
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, true);
EXPECT_EQ(result.get()->error_message, "");
EXPECT_EQ(result.get()->full_node_name, "/test_component_bar");
EXPECT_EQ(result.get()->unique_id, 2u);
}
// Test remapping the node name
{
auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
request->package_name = "rclcpp_components";
request->plugin_name = "test_rclcpp_components::TestComponentFoo";
request->node_name = "test_component_baz";
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, true);
EXPECT_EQ(result.get()->error_message, "");
EXPECT_EQ(result.get()->full_node_name, "/test_component_baz");
EXPECT_EQ(result.get()->unique_id, 3u);
}
// Test remapping the node namespace
{
auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
request->package_name = "rclcpp_components";
request->plugin_name = "test_rclcpp_components::TestComponentFoo";
request->node_namespace = "/ns";
request->node_name = "test_component_bing";
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, true);
EXPECT_EQ(result.get()->error_message, "");
EXPECT_EQ(result.get()->full_node_name, "/ns/test_component_bing");
EXPECT_EQ(result.get()->unique_id, 4u);
}
auto node_names = node->get_node_names();
auto find_in_nodes = [node_names](std::string name) {
return std::find(node_names.begin(), node_names.end(), name) != node_names.end();
};
EXPECT_TRUE(find_in_nodes("test_component_foo"));
EXPECT_TRUE(find_in_nodes("test_component_bar"));
EXPECT_TRUE(find_in_nodes("test_component_baz"));
EXPECT_TRUE(find_in_nodes("test_component_bing"));
}
TEST_F(TestComponentManager, load_invalid_components)
{
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
auto node = rclcpp::Node::make_shared("test_component_manager");
auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
exec->add_node(manager);
exec->add_node(node);
auto client = node->create_client<composition_interfaces::srv::LoadNode>(
"/ComponentManager/_container/load_node");
if (!client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}
{
// Valid package, but invalid class name.
auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
request->package_name = "rclcpp_components";
request->plugin_name = "test_rclcpp_components::TestComponent";
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, false);
EXPECT_EQ(result.get()->error_message, "Failed to find class with the requested plugin name.");
EXPECT_EQ(result.get()->full_node_name, "");
EXPECT_EQ(result.get()->unique_id, 0u);
}
{
// Invalid package, but valid class name.
auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
request->package_name = "rclcpp_components_foo";
request->plugin_name = "test_rclcpp_components::TestComponentFoo";
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, false);
EXPECT_EQ(result.get()->error_message, "Could not find requested resource in ament index");
EXPECT_EQ(result.get()->full_node_name, "");
EXPECT_EQ(result.get()->unique_id, 0u);
}
}
TEST_F(TestComponentManager, list_components)
{
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
auto node = rclcpp::Node::make_shared("test_component_manager");
auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
exec->add_node(manager);
exec->add_node(node);
{
auto client = node->create_client<composition_interfaces::srv::LoadNode>(
"/ComponentManager/_container/load_node");
if (!client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}
{
auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
request->package_name = "rclcpp_components";
request->plugin_name = "test_rclcpp_components::TestComponentFoo";
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, true);
EXPECT_EQ(result.get()->error_message, "");
EXPECT_EQ(result.get()->full_node_name, "/test_component_foo");
EXPECT_EQ(result.get()->unique_id, 1u);
}
}
{
auto client = node->create_client<composition_interfaces::srv::ListNodes>(
"/ComponentManager/_container/list_nodes");
if (!client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}
{
auto request = std::make_shared<composition_interfaces::srv::ListNodes::Request>();
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
auto node_names = result.get()->full_node_names;
auto unique_ids = result.get()->unique_ids;
EXPECT_EQ(node_names.size(), 1u);
EXPECT_EQ(node_names[0], "/test_component_foo");
EXPECT_EQ(unique_ids.size(), 1u);
EXPECT_EQ(unique_ids[0], 1u);
}
}
}
TEST_F(TestComponentManager, unload_component)
{
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
auto node = rclcpp::Node::make_shared("test_component_manager");
auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
exec->add_node(manager);
exec->add_node(node);
{
auto client = node->create_client<composition_interfaces::srv::LoadNode>(
"/ComponentManager/_container/load_node");
if (!client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}
{
auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
request->package_name = "rclcpp_components";
request->plugin_name = "test_rclcpp_components::TestComponentFoo";
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, true);
EXPECT_EQ(result.get()->error_message, "");
EXPECT_EQ(result.get()->full_node_name, "/test_component_foo");
EXPECT_EQ(result.get()->unique_id, 1u);
}
}
auto node_names = node->get_node_names();
auto find_in_nodes = [node_names](std::string name) {
return std::find(node_names.begin(), node_names.end(), name) != node_names.end();
};
EXPECT_TRUE(find_in_nodes("test_component_foo"));
{
auto client = node->create_client<composition_interfaces::srv::UnloadNode>(
"/ComponentManager/_container/unload_node");
if (!client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}
{
auto request = std::make_shared<composition_interfaces::srv::UnloadNode::Request>();
request->unique_id = 1;
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, true);
EXPECT_EQ(result.get()->error_message, "");
}
{
auto request = std::make_shared<composition_interfaces::srv::UnloadNode::Request>();
request->unique_id = 1;
auto result = client->async_send_request(request);
auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result.
EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
EXPECT_EQ(result.get()->success, false);
EXPECT_EQ(result.get()->error_message, "No node found with unique_id: 1");
}
}
}