From 2774c79c004b88ae7f6045f8c32876f84dbcfa77 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 12 Mar 2019 14:11:02 -0500 Subject: [PATCH 01/14] Introduce rclcpp_components package Signed-off-by: Michael Carroll --- rclcpp_components/CMakeLists.txt | 88 ++++++++ .../rclcpp_components_package_hook.cmake | 18 ++ .../rclcpp_components_register_nodes.cmake | 62 ++++++ .../rclcpp_components/node_factory.hpp | 37 ++++ .../node_factory_template.hpp | 44 ++++ .../node_instance_wrapper.hpp | 55 +++++ .../rclcpp_components/register_node_macro.hpp | 26 +++ rclcpp_components/package.xml | 32 +++ .../rclcpp_components-extras.cmake | 29 +++ rclcpp_components/src/component_container.cpp | 30 +++ rclcpp_components/src/component_manager.cpp | 192 ++++++++++++++++++ rclcpp_components/src/component_manager.hpp | 74 +++++++ rclcpp_components/src/filesystem_helper.hpp | 188 +++++++++++++++++ rclcpp_components/src/split.hpp | 71 +++++++ rclcpp_components/test/talker_component.cpp | 53 +++++ 15 files changed, 999 insertions(+) create mode 100644 rclcpp_components/CMakeLists.txt create mode 100644 rclcpp_components/cmake/rclcpp_components_package_hook.cmake create mode 100644 rclcpp_components/cmake/rclcpp_components_register_nodes.cmake create mode 100644 rclcpp_components/include/rclcpp_components/node_factory.hpp create mode 100644 rclcpp_components/include/rclcpp_components/node_factory_template.hpp create mode 100644 rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp create mode 100644 rclcpp_components/include/rclcpp_components/register_node_macro.hpp create mode 100644 rclcpp_components/package.xml create mode 100644 rclcpp_components/rclcpp_components-extras.cmake create mode 100644 rclcpp_components/src/component_container.cpp create mode 100644 rclcpp_components/src/component_manager.cpp create mode 100644 rclcpp_components/src/component_manager.hpp create mode 100644 rclcpp_components/src/filesystem_helper.hpp create mode 100644 rclcpp_components/src/split.hpp create mode 100644 rclcpp_components/test/talker_component.cpp diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt new file mode 100644 index 0000000000..d75182f62e --- /dev/null +++ b/rclcpp_components/CMakeLists.txt @@ -0,0 +1,88 @@ +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) + +include_directories(include) + +add_executable( + component_container + src/component_manager.cpp + src/component_container.cpp +) + +ament_target_dependencies(component_container + "ament_index_cpp" + "class_loader" + "composition_interfaces" + "rclcpp" +) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + target_link_libraries(component_container "stdc++fs") +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(std_msgs REQUIRED) + + set(node_plugins "") + + add_library( + test_components + SHARED + test/talker_component.cpp + ) + ament_target_dependencies( + test_components + "class_loader" + "rclcpp" + "std_msgs" + ) + set(node_plugins "${node_plugins}test_rclcpp_components::Talker;$\n") + + file(GENERATE + OUTPUT + "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$/share/ament_index/resource_index/rclcpp_components/${PROJECT_NAME}" + CONTENT "${node_plugins}") + +endif() + +# Install executables +install( + TARGETS component_container + 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) diff --git a/rclcpp_components/cmake/rclcpp_components_package_hook.cmake b/rclcpp_components/cmake/rclcpp_components_package_hook.cmake new file mode 100644 index 0000000000..4833d02a71 --- /dev/null +++ b/rclcpp_components/cmake/rclcpp_components_package_hook.cmake @@ -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}") + diff --git a/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake b/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake new file mode 100644 index 0000000000..e8f5ebcc50 --- /dev/null +++ b/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake @@ -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 +# +# :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}/$\n") + endforeach() + endif() +endmacro() + diff --git a/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp_components/include/rclcpp_components/node_factory.hpp new file mode 100644 index 0000000000..39e46d70b5 --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -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__NODE_FACTORY_HPP__ +#define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ + +#include "rclcpp_components/node_instance_wrapper.hpp" + +namespace rclcpp_components +{ +class NodeFactory +{ +public: + NodeFactory() = default; + + virtual ~NodeFactory() = default; + + virtual + NodeInstanceWrapper + create_node_instance(std::string node_name, + std::string node_namespace, + rclcpp::NodeOptions options) = 0; +}; +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ diff --git a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp new file mode 100644 index 0000000000..1fa0943454 --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -0,0 +1,44 @@ +// 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 "rclcpp_components/node_factory.hpp" + +namespace rclcpp_components +{ + +template +class NodeFactoryTemplate : public NodeFactory +{ +public: + NodeFactoryTemplate() = default; + virtual ~NodeFactoryTemplate() = default; + + virtual + NodeInstanceWrapper + create_node_instance(std::string node_name, + std::string node_namespace, + rclcpp::NodeOptions options) + { + auto node = std::make_shared(node_name, node_namespace, options); + + return NodeInstanceWrapper(node, + std::bind(&NodeT::get_node_base_interface, node)); + } +}; +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ diff --git a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp new file mode 100644 index 0000000000..76539b47d8 --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp @@ -0,0 +1,55 @@ +// 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 +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" + +namespace rclcpp_components +{ +class NodeInstanceWrapper +{ +public: + using NodeBaseInterfaceGetter = std::function< + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr(const std::shared_ptr &)>; + + NodeInstanceWrapper( + std::shared_ptr node_instance, + NodeBaseInterfaceGetter node_base_interface_getter) + : node_instance_(node_instance), node_base_interface_getter_(node_base_interface_getter) + {} + + const std::shared_ptr + get_node_instance() const + { + return node_instance_; + } + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() + { + return node_base_interface_getter_(node_instance_); + } + +private: + std::shared_ptr node_instance_; + NodeBaseInterfaceGetter node_base_interface_getter_; +}; +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__ diff --git a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp new file mode 100644 index 0000000000..087ca5389f --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp @@ -0,0 +1,26 @@ +// 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" + +#define RCLCPP_COMPONENTS_REGISTER_NODE(NodeClass) \ + CLASS_LOADER_REGISTER_CLASS(rclcpp_components::NodeFactoryTemplate, \ + rclcpp_components::NodeFactory) + +#endif // RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ + diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml new file mode 100644 index 0000000000..00f34b3457 --- /dev/null +++ b/rclcpp_components/package.xml @@ -0,0 +1,32 @@ + + + + rclcpp_components + 0.6.2 + Package containing container tools for dynamically loadable components + Michael Carroll + Apache License 2.0 + + ament_cmake_ros + + ament_index_cpp + class_loader + composition_interfaces + rclcpp + + ament_index_cpp + class_loader + composition_interfaces + rclcpp + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + launch_testing + std_msgs + + + ament_cmake + + + diff --git a/rclcpp_components/rclcpp_components-extras.cmake b/rclcpp_components/rclcpp_components-extras.cmake new file mode 100644 index 0000000000..23f0f28934 --- /dev/null +++ b/rclcpp_components/rclcpp_components-extras.cmake @@ -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") + diff --git a/rclcpp_components/src/component_container.cpp b/rclcpp_components/src/component_container.cpp new file mode 100644 index 0000000000..3a098f5a4f --- /dev/null +++ b/rclcpp_components/src/component_container.cpp @@ -0,0 +1,30 @@ +// 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 + +#include "rclcpp/rclcpp.hpp" + +#include "component_manager.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + + auto node = std::make_shared(&exec); + exec.add_node(node); + + exec.spin(); +} diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp new file mode 100644 index 0000000000..4b88cfaee1 --- /dev/null +++ b/rclcpp_components/src/component_manager.cpp @@ -0,0 +1,192 @@ +// 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 "filesystem_helper.hpp" +#include "split.hpp" + +using namespace std::placeholders; + +namespace rclcpp_components +{ + +ComponentManager::ComponentManager(rclcpp::executor::Executor * executor) +: Node("ComponentManager"), + executor_(executor) +{ + loadNode_srv_ = create_service("_container/load_node", + std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3)); + unloadNode_srv_ = create_service("_container/unload_node", + std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3)); + listNodes_srv_ = create_service("_container/list_nodes", + std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3)); +} + +void +ComponentManager::OnLoadNode( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void) request_header; + + std::string content; + std::string base_path; + if (!ament_index_cpp::get_resource( + "rclcpp_components", request->package_name, content, &base_path)) + { + RCLCPP_ERROR(get_logger(), "Could not find requested resource in ament index"); + response->error_message = "Could not find requested resource in ament index"; + response->success = false; + return; + } + + std::vector lines = split(content, '\n', true); + for (auto line : lines) { + std::vector parts = split(line, ';'); + if (parts.size() != 2) { + RCLCPP_ERROR(get_logger(), "Invalid resource entry"); + response->error_message = "Invalid resource entry"; + response->success = false; + return; + } + // match plugin name with the same rmw suffix as this executable + if (parts[0] != request->plugin_name) { + continue; + } + + std::string class_name = parts[0]; + std::string fq_class_name = "rclcpp_components::NodeFactoryTemplate<" + class_name + ">"; + + // load node plugin + std::string library_path = parts[1]; + if (!fs::path(library_path).is_absolute()) { + library_path = base_path + "/" + library_path; + } + class_loader::ClassLoader * loader; + + if (loaders_.find(library_path) != loaders_.end()) { + loader = loaders_[library_path]; + } else { + RCLCPP_INFO(get_logger(), "Load library %s", library_path.c_str()); + try { + loader = new class_loader::ClassLoader(library_path); + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Failed to load library: %s", ex.what()); + response->error_message = "Failed to load library"; + response->success = false; + return; + } catch (...) { + RCLCPP_ERROR(get_logger(), "Failed to load library"); + response->error_message = "Failed to load library"; + response->success = false; + return; + } + } + auto classes = loader->getAvailableClasses(); + for (auto clazz : classes) { + RCLCPP_INFO(get_logger(), "Found class %s", class_name.c_str()); + if (clazz == class_name || clazz == fq_class_name) { + RCLCPP_INFO(get_logger(), "Instantiate class %s", class_name.c_str()); + loaders_[library_path] = loader; + + auto node_factory = loader->createInstance(clazz); + + std::vector parameters; + for (auto & p: request->parameters) + { + parameters.push_back(rclcpp::Parameter::from_parameter_msg(p)); + } + + auto options = rclcpp::NodeOptions() + .initial_parameters(parameters) + .arguments(request->remap_rules); + + auto node_id = unique_id++; + + auto node_wrapper = node_factory->create_node_instance( + request->node_name, + request->node_namespace, + options); + + auto node = node_wrapper.get_node_base_interface(); + nodes_[node_id] = node; + + response->full_node_name = node->get_fully_qualified_name(); + response->unique_id = node_id; + response->success = true; + executor_->add_node(nodes_[node_id], true); + return; + } + } + + // no matching class found in loader + delete loader; + RCLCPP_ERROR( + get_logger(), "Failed to find class with the requested plugin name '%s' in " + "the loaded library", + request->plugin_name.c_str()); + response->success = false; + return; + } + + RCLCPP_ERROR( + get_logger(), "Failed to find plugin name '%s' in prefix '%s'", + request->plugin_name.c_str(), base_path.c_str()); + response->success = false; +} + +void +ComponentManager::OnUnloadNode( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void) request_header; + + auto node = nodes_.find(request->unique_id); + + if(node == nodes_.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 + { + executor_->remove_node(node->second); + nodes_.erase(node); + response->success = true; + } +} + +void +ComponentManager::OnListNodes( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void) request_header; + (void) request; + + for(const auto& node: nodes_) + { + response->unique_ids.push_back(node.first); + response->full_node_names.push_back(node.second->get_fully_qualified_name()); + } +} + +} // namespace rclcpp_components diff --git a/rclcpp_components/src/component_manager.hpp b/rclcpp_components/src/component_manager.hpp new file mode 100644 index 0000000000..576086b434 --- /dev/null +++ b/rclcpp_components/src/component_manager.hpp @@ -0,0 +1,74 @@ +// 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__COMPONENT_MANAGER_HPP__ +#define RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ + +#include "ament_index_cpp/get_resource.hpp" +#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 ComponentManager : public rclcpp::Node +{ +public: + using LoadNode = composition_interfaces::srv::LoadNode; + using UnloadNode = composition_interfaces::srv::UnloadNode; + using ListNodes = composition_interfaces::srv::ListNodes; + + ComponentManager(rclcpp::executor::Executor * executor); + +private: + void OnLoadNode( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + void OnUnloadNode( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + void OnListNodes( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + +private: + rclcpp::executor::Executor * executor_; + + std::map nodes_; + std::map loaders_; + + uint64_t unique_id {1}; + + rclcpp::Service::SharedPtr loadNode_srv_; + rclcpp::Service::SharedPtr unloadNode_srv_; + rclcpp::Service::SharedPtr listNodes_srv_; +}; + +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ diff --git a/rclcpp_components/src/filesystem_helper.hpp b/rclcpp_components/src/filesystem_helper.hpp new file mode 100644 index 0000000000..524b2a2912 --- /dev/null +++ b/rclcpp_components/src/filesystem_helper.hpp @@ -0,0 +1,188 @@ +/* + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/// Includes std::filesystem and aliases the namespace to `pluginlib::impl::fs`. +/** + * If std::filesystem is not available the necessary functions are emulated. + */ + +#ifndef RCLCPP_COMPONENTS__FILESYSTEM_HELPER_HPP_ +#define RCLCPP_COMPONETNS__FILESYSTEM_HELPER_HPP_ + +#if defined(_MSC_VER) +# if _MSC_VER >= 1900 +# include +namespace rclcpp_components +{ +namespace fs = std::experimental::filesystem; +} // namespace components + +# define RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM +# endif + +#elif defined(__has_include) +# if __has_include() && __cplusplus >= 201703L +# include + +namespace rclcpp_components +{ +namespace fs = std::filesystem; +} // namespace rclcpp_components + +# define RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM +# elif __has_include() +# include + +namespace rclcpp_components +{ +namespace fs = std::experimental::filesystem; +} // namespace rclcpp_components + +# define RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM +# endif +#endif + +// The standard library does not provide it, so emulate it. +#ifndef RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM + +#include +#include + +#ifdef _WIN32 +#define CLASS_LOADER_IMPL_OS_DIRSEP '\\' +#else +#define CLASS_LOADER_IMPL_OS_DIRSEP '/' +#endif + +#ifdef _WIN32 + #include + #define access _access_s +#else + #include +#endif + +#include "./split.hpp" + +namespace rclcpp_components +{ +namespace fs +{ + +class path +{ +public: + static constexpr char preferred_separator = CLASS_LOADER_IMPL_OS_DIRSEP; + + path() + : path("") + {} + + path(const std::string & p) // NOLINT(runtime/explicit): this is a conversion constructor + : path_(p), path_as_vector_(split(p, std::string(1, preferred_separator))) + {} + + std::string string() const + { + return path_; + } + + bool exists() const + { + return access(path_.c_str(), 0) == 0; + } + + std::vector::const_iterator cbegin() const + { + return path_as_vector_.cbegin(); + } + + std::vector::const_iterator cend() const + { + return path_as_vector_.cend(); + } + + path parent_path() const + { + path parent(""); + for (auto it = this->cbegin(); it != --this->cend(); ++it) { + parent /= *it; + } + return parent; + } + + path filename() const + { + return path_.empty() ? path() : *--this->cend(); + } + + path operator/(const std::string & other) + { + return this->operator/(path(other)); + } + + path & operator/=(const std::string & other) + { + this->operator/=(path(other)); + return *this; + } + + path operator/(const path & other) + { + return path(*this).operator/=(other); + } + + path & operator/=(const path & other) + { + this->path_ += CLASS_LOADER_IMPL_OS_DIRSEP + other.string(); + this->path_as_vector_.insert( + std::end(this->path_as_vector_), + std::begin(other.path_as_vector_), std::end(other.path_as_vector_)); + return *this; + } + +private: + std::string path_; + std::vector path_as_vector_; +}; + +inline bool exists(const path & path_to_check) +{ + return path_to_check.exists(); +} + +#undef CLASS_LOADER_IMPL_OS_DIRSEP + +} // namespace fs +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM + +#undef RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM + +#endif // RCLCPP_COMPONENTS__FILESYSTEM_HELPER_HPP_ diff --git a/rclcpp_components/src/split.hpp b/rclcpp_components/src/split.hpp new file mode 100644 index 0000000000..9a0da15a23 --- /dev/null +++ b/rclcpp_components/src/split.hpp @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2017, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef RCLCPP_COMPONENTS__SPLIT_HPP_ +#define RCLCPP_COMPONETNS__SPLIT_HPP_ + +#include +#include +#include + +namespace rclcpp_components +{ + +// Version used in the filesystem helper +inline std::vector +split(const std::string & input, const std::string & regex) +{ + std::regex re(regex); + // the -1 will cause this to return the stuff between the matches, see the submatch argument: + // http://en.cppreference.com/w/cpp/regex/regex_token_iterator/regex_token_iterator + std::sregex_token_iterator first(input.begin(), input.end(), re, -1); + std::sregex_token_iterator last; + return {first, last}; // vector will copy from first to last +} + +// Version used in parsing ament_index +std::vector split( + const std::string & s, char delim, bool skip_empty = false) +{ + std::vector result; + std::stringstream ss; + ss.str(s); + std::string item; + while (std::getline(ss, item, delim)) { + if (skip_empty && item == "") { + continue; + } + result.push_back(item); + } + return result; +} + +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__SPLIT_HPP_ diff --git a/rclcpp_components/test/talker_component.cpp b/rclcpp_components/test/talker_component.cpp new file mode 100644 index 0000000000..b24d856b89 --- /dev/null +++ b/rclcpp_components/test/talker_component.cpp @@ -0,0 +1,53 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +namespace test_rclcpp_components +{ +class Talker : public rclcpp::Node +{ +public: + Talker(std::string node_name, std::string node_namespace, rclcpp::NodeOptions options) + : Node(node_name, node_namespace, options), + count_(0) + { + // Create a publisher of "std_mgs/String" messages on the "chatter" topic. + pub_ = create_publisher("chatter"); + + if(!get_parameter("message", message_)) + { + message_ = "Hello World: "; + } + + // Use a timer to schedule periodic message publishing. + timer_ = create_wall_timer(1s, std::bind(&Talker::on_timer, this)); + } + + void on_timer() + { + auto msg = std::make_shared(); + msg->data = message_ + std::to_string(++count_); + std::cerr << "Publishing" << std::endl; + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg->data.c_str()); + std::flush(std::cout); + + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + pub_->publish(msg); + } + +private: + std::string message_; + size_t count_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::Talker); From 843d05581104d3e846d24d48367f322ef5112592 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 21 Mar 2019 16:45:34 -0500 Subject: [PATCH 02/14] Keep pointer to NodeWrapper vs NodeInterface. Signed-off-by: Michael Carroll --- .../node_instance_wrapper.hpp | 4 +++ rclcpp_components/src/component_container.cpp | 10 +++--- rclcpp_components/src/component_manager.cpp | 31 +++++++++++-------- rclcpp_components/src/component_manager.hpp | 6 ++-- rclcpp_components/test/talker_component.cpp | 3 +- 5 files changed, 30 insertions(+), 24 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp index 76539b47d8..2cd9e3f081 100644 --- a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp +++ b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp @@ -28,6 +28,10 @@ class NodeInstanceWrapper using NodeBaseInterfaceGetter = std::function< rclcpp::node_interfaces::NodeBaseInterface::SharedPtr(const std::shared_ptr &)>; + NodeInstanceWrapper(): + node_instance_(nullptr) + {} + NodeInstanceWrapper( std::shared_ptr node_instance, NodeBaseInterfaceGetter node_base_interface_getter) diff --git a/rclcpp_components/src/component_container.cpp b/rclcpp_components/src/component_container.cpp index 3a098f5a4f..f45fcadb78 100644 --- a/rclcpp_components/src/component_container.cpp +++ b/rclcpp_components/src/component_container.cpp @@ -21,10 +21,8 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor exec; - - auto node = std::make_shared(&exec); - exec.add_node(node); - - exec.spin(); + auto exec = std::make_shared(); + auto node = std::make_shared(exec); + exec->add_node(node); + exec->spin(); } diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 4b88cfaee1..8c2a3c7a9f 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -21,7 +21,7 @@ using namespace std::placeholders; namespace rclcpp_components { -ComponentManager::ComponentManager(rclcpp::executor::Executor * executor) +ComponentManager::ComponentManager(std::weak_ptr executor) : Node("ComponentManager"), executor_(executor) { @@ -115,18 +115,19 @@ ComponentManager::OnLoadNode( auto node_id = unique_id++; - auto node_wrapper = node_factory->create_node_instance( + node_wrappers_[node_id] = node_factory->create_node_instance( request->node_name, request->node_namespace, options); - auto node = node_wrapper.get_node_base_interface(); - nodes_[node_id] = node; - + 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; - executor_->add_node(nodes_[node_id], true); return; } } @@ -155,9 +156,9 @@ ComponentManager::OnUnloadNode( { (void) request_header; - auto node = nodes_.find(request->unique_id); + auto wrapper = node_wrappers_.find(request->unique_id); - if(node == nodes_.end()) + if(wrapper == node_wrappers_.end()) { response->success = false; std::stringstream ss; @@ -167,8 +168,11 @@ ComponentManager::OnUnloadNode( } else { - executor_->remove_node(node->second); - nodes_.erase(node); + if (auto exec = executor_.lock()) + { + exec->remove_node(wrapper->second.get_node_base_interface()); + } + node_wrappers_.erase(wrapper); response->success = true; } } @@ -182,10 +186,11 @@ ComponentManager::OnListNodes( (void) request_header; (void) request; - for(const auto& node: nodes_) + for(auto& wrapper: node_wrappers_) { - response->unique_ids.push_back(node.first); - response->full_node_names.push_back(node.second->get_fully_qualified_name()); + response->unique_ids.push_back(wrapper.first); + response->full_node_names.push_back( + wrapper.second.get_node_base_interface()->get_fully_qualified_name()); } } diff --git a/rclcpp_components/src/component_manager.hpp b/rclcpp_components/src/component_manager.hpp index 576086b434..be91601f62 100644 --- a/rclcpp_components/src/component_manager.hpp +++ b/rclcpp_components/src/component_manager.hpp @@ -38,7 +38,7 @@ class ComponentManager : public rclcpp::Node using UnloadNode = composition_interfaces::srv::UnloadNode; using ListNodes = composition_interfaces::srv::ListNodes; - ComponentManager(rclcpp::executor::Executor * executor); + ComponentManager(std::weak_ptr executor); private: void OnLoadNode( @@ -57,9 +57,9 @@ class ComponentManager : public rclcpp::Node std::shared_ptr response); private: - rclcpp::executor::Executor * executor_; + std::weak_ptr executor_; - std::map nodes_; + std::map node_wrappers_; std::map loaders_; uint64_t unique_id {1}; diff --git a/rclcpp_components/test/talker_component.cpp b/rclcpp_components/test/talker_component.cpp index b24d856b89..297eb2ce88 100644 --- a/rclcpp_components/test/talker_component.cpp +++ b/rclcpp_components/test/talker_component.cpp @@ -31,7 +31,6 @@ class Talker : public rclcpp::Node { auto msg = std::make_shared(); msg->data = message_ + std::to_string(++count_); - std::cerr << "Publishing" << std::endl; RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg->data.c_str()); std::flush(std::cout); @@ -50,4 +49,4 @@ class Talker : public rclcpp::Node #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::Talker); +RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::Talker) From 39ee62e1299ad20a26296984a306d81fb246b676 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 22 Mar 2019 15:43:51 -0500 Subject: [PATCH 03/14] Remove component registration from rclcpp Signed-off-by: Michael Carroll --- rclcpp/CMakeLists.txt | 9 +-- rclcpp/cmake/rclcpp_create_node_main.cmake | 26 -------- rclcpp/cmake/rclcpp_package_hook.cmake | 17 ------ .../cmake/rclcpp_register_node_plugins.cmake | 61 ------------------- rclcpp/rclcpp-extras.cmake | 29 --------- 5 files changed, 1 insertion(+), 141 deletions(-) delete mode 100644 rclcpp/cmake/rclcpp_create_node_main.cmake delete mode 100644 rclcpp/cmake/rclcpp_package_hook.cmake delete mode 100644 rclcpp/cmake/rclcpp_register_node_plugins.cmake delete mode 100644 rclcpp/rclcpp-extras.cmake diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index df866a99c6..683792d384 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -432,14 +432,7 @@ if(BUILD_TESTING) endif() endif() -ament_package( - CONFIG_EXTRAS rclcpp-extras.cmake -) - -install( - DIRECTORY cmake - DESTINATION share/${PROJECT_NAME} -) +ament_package() install( DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/ diff --git a/rclcpp/cmake/rclcpp_create_node_main.cmake b/rclcpp/cmake/rclcpp_create_node_main.cmake deleted file mode 100644 index 086d5f6f13..0000000000 --- a/rclcpp/cmake/rclcpp_create_node_main.cmake +++ /dev/null @@ -1,26 +0,0 @@ -# Copyright 2015 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. - - -set(rclcpp_node_main_SRC "${rclcpp_DIR}/../../../src/rclcpp/node_main.cpp") - -function(rclcpp_create_node_main node_library_target) - if(NOT TARGET ${node_library_target}) - message(FATAL_ERROR "rclcpp_create_node_main() the first argument must be a valid target name") - endif() - set(executable_name_ ${node_library_target}_node) - add_executable(${executable_name_} ${rclcpp_node_main_SRC}) - target_link_libraries(${executable_name_} ${node_library_target}) - install(TARGETS ${executable_name_} DESTINATION bin) -endfunction() diff --git a/rclcpp/cmake/rclcpp_package_hook.cmake b/rclcpp/cmake/rclcpp_package_hook.cmake deleted file mode 100644 index 9e73620f79..0000000000 --- a/rclcpp/cmake/rclcpp_package_hook.cmake +++ /dev/null @@ -1,17 +0,0 @@ -# Copyright 2016 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( - "node_plugin" CONTENT "${_RCLCPP__NODE_PLUGINS}") diff --git a/rclcpp/cmake/rclcpp_register_node_plugins.cmake b/rclcpp/cmake/rclcpp_register_node_plugins.cmake deleted file mode 100644 index f8675f823f..0000000000 --- a/rclcpp/cmake/rclcpp_register_node_plugins.cmake +++ /dev/null @@ -1,61 +0,0 @@ -# Copyright 2016 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 a node plugin with the ament resource index. -# -# The passed library can contain multiple plugins extending the node interface. -# -# :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_register_node_plugins target) - if(NOT TARGET ${target}) - message( - FATAL_ERROR - "rclcpp_register_node_plugins() 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_register_node_plugins() first argument " - "'${target}' is not a shared library target") - endif() - - if(${ARGC} GREATER 0) - _rclcpp_register_package_hook() - set(_unique_names) - foreach(_arg ${ARGN}) - if(_arg IN_LIST _unique_names) - message( - FATAL_ERROR - "rclcpp_register_node_plugins() 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__NODE_PLUGINS - "${_RCLCPP__NODE_PLUGINS}${_arg};${_path}/$\n") - endforeach() - endif() -endmacro() diff --git a/rclcpp/rclcpp-extras.cmake b/rclcpp/rclcpp-extras.cmake deleted file mode 100644 index 7d3a1429fa..0000000000 --- a/rclcpp/rclcpp-extras.cmake +++ /dev/null @@ -1,29 +0,0 @@ -# Copyright 2015 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/rclcpp-extras.cmake - -# register ament_package() hook for node plugins once -macro(_rclcpp_register_package_hook) - if(NOT DEFINED _RCLCPP_PACKAGE_HOOK_REGISTERED) - set(_RCLCPP_PACKAGE_HOOK_REGISTERED TRUE) - - find_package(ament_cmake_core QUIET REQUIRED) - ament_register_extension("ament_package" "rclcpp" - "rclcpp_package_hook.cmake") - endif() -endmacro() - -include("${rclcpp_DIR}/rclcpp_create_node_main.cmake") -include("${rclcpp_DIR}/rclcpp_register_node_plugins.cmake") From ba5ad832b07d336315e38538c26e3d59e550c98d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 25 Mar 2019 11:08:19 -0500 Subject: [PATCH 04/14] Make topics names private-prefix. Signed-off-by: Michael Carroll --- rclcpp_components/src/component_manager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 8c2a3c7a9f..bde677e7f3 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -25,11 +25,11 @@ ComponentManager::ComponentManager(std::weak_ptr exe : Node("ComponentManager"), executor_(executor) { - loadNode_srv_ = create_service("_container/load_node", + loadNode_srv_ = create_service("~/_container/load_node", std::bind(&ComponentManager::OnLoadNode, this, _1, _2, _3)); - unloadNode_srv_ = create_service("_container/unload_node", + unloadNode_srv_ = create_service("~/_container/unload_node", std::bind(&ComponentManager::OnUnloadNode, this, _1, _2, _3)); - listNodes_srv_ = create_service("_container/list_nodes", + listNodes_srv_ = create_service("~/_container/list_nodes", std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3)); } From 115ae5de6abcaeb0ae0b1ce60aea7fbc9bf9ac00 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 25 Mar 2019 11:08:49 -0500 Subject: [PATCH 05/14] Handle name and namespace with remap rules. Signed-off-by: Michael Carroll --- .../rclcpp_components/node_factory.hpp | 4 +--- .../node_factory_template.hpp | 6 ++---- rclcpp_components/src/component_manager.cpp | 19 ++++++++++++++----- rclcpp_components/test/talker_component.cpp | 4 ++-- 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp_components/include/rclcpp_components/node_factory.hpp index 39e46d70b5..0600a482ec 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -28,9 +28,7 @@ class NodeFactory virtual NodeInstanceWrapper - create_node_instance(std::string node_name, - std::string node_namespace, - rclcpp::NodeOptions options) = 0; + create_node_instance(rclcpp::NodeOptions options) = 0; }; } // namespace rclcpp_components diff --git a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp index 1fa0943454..4d5c2401e5 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -29,11 +29,9 @@ class NodeFactoryTemplate : public NodeFactory virtual NodeInstanceWrapper - create_node_instance(std::string node_name, - std::string node_namespace, - rclcpp::NodeOptions options) + create_node_instance(rclcpp::NodeOptions options) { - auto node = std::make_shared(node_name, node_namespace, options); + auto node = std::make_shared(options); return NodeInstanceWrapper(node, std::bind(&NodeT::get_node_base_interface, node)); diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index bde677e7f3..5cd35000ec 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -109,16 +109,25 @@ ComponentManager::OnLoadNode( parameters.push_back(rclcpp::Parameter::from_parameter_msg(p)); } + std::vector remap_rules { request->remap_rules }; + + if (request->node_name.length()) + { + remap_rules.push_back("__node:=" + request->node_name); + } + + if (request->node_namespace.length()) + { + remap_rules.push_back("__ns:=" + request->node_namespace); + } + auto options = rclcpp::NodeOptions() .initial_parameters(parameters) - .arguments(request->remap_rules); + .arguments(remap_rules); auto node_id = unique_id++; - node_wrappers_[node_id] = node_factory->create_node_instance( - request->node_name, - request->node_namespace, - options); + node_wrappers_[node_id] = node_factory->create_node_instance(options); auto node = node_wrappers_[node_id].get_node_base_interface(); if (auto exec = executor_.lock()) diff --git a/rclcpp_components/test/talker_component.cpp b/rclcpp_components/test/talker_component.cpp index 297eb2ce88..909b951462 100644 --- a/rclcpp_components/test/talker_component.cpp +++ b/rclcpp_components/test/talker_component.cpp @@ -11,8 +11,8 @@ namespace test_rclcpp_components class Talker : public rclcpp::Node { public: - Talker(std::string node_name, std::string node_namespace, rclcpp::NodeOptions options) - : Node(node_name, node_namespace, options), + Talker(rclcpp::NodeOptions options) + : Node("talker", options), count_(0) { // Create a publisher of "std_mgs/String" messages on the "chatter" topic. From 60df764a24835642de3bd4daa29074b40cb50400 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 26 Mar 2019 14:51:13 -0500 Subject: [PATCH 06/14] Linting. Signed-off-by: Michael Carroll --- rclcpp_components/CMakeLists.txt | 23 ------ .../node_factory_template.hpp | 2 + .../node_instance_wrapper.hpp | 4 +- .../rclcpp_components/register_node_macro.hpp | 3 +- rclcpp_components/src/component_manager.cpp | 33 ++++----- rclcpp_components/src/component_manager.hpp | 12 ++-- rclcpp_components/src/filesystem_helper.hpp | 71 ++++++++++--------- rclcpp_components/src/split.hpp | 68 ++++++++++-------- rclcpp_components/test/talker_component.cpp | 52 -------------- 9 files changed, 103 insertions(+), 165 deletions(-) delete mode 100644 rclcpp_components/test/talker_component.cpp diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index d75182f62e..9b65f84360 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -38,29 +38,6 @@ endif() if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - - find_package(std_msgs REQUIRED) - - set(node_plugins "") - - add_library( - test_components - SHARED - test/talker_component.cpp - ) - ament_target_dependencies( - test_components - "class_loader" - "rclcpp" - "std_msgs" - ) - set(node_plugins "${node_plugins}test_rclcpp_components::Talker;$\n") - - file(GENERATE - OUTPUT - "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$/share/ament_index/resource_index/rclcpp_components/${PROJECT_NAME}" - CONTENT "${node_plugins}") - endif() # Install executables diff --git a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp index 4d5c2401e5..ef1e94652e 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ #define RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ +#include + #include "rclcpp_components/node_factory.hpp" namespace rclcpp_components diff --git a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp index 2cd9e3f081..3dbe076942 100644 --- a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp +++ b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp @@ -28,8 +28,8 @@ class NodeInstanceWrapper using NodeBaseInterfaceGetter = std::function< rclcpp::node_interfaces::NodeBaseInterface::SharedPtr(const std::shared_ptr &)>; - NodeInstanceWrapper(): - node_instance_(nullptr) + NodeInstanceWrapper() + : node_instance_(nullptr) {} NodeInstanceWrapper( diff --git a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp index 087ca5389f..794b250105 100644 --- a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp +++ b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp @@ -20,7 +20,6 @@ #define RCLCPP_COMPONENTS_REGISTER_NODE(NodeClass) \ CLASS_LOADER_REGISTER_CLASS(rclcpp_components::NodeFactoryTemplate, \ - rclcpp_components::NodeFactory) + rclcpp_components::NodeFactory) #endif // RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ - diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 5cd35000ec..b786cadc61 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include + #include "component_manager.hpp" #include "filesystem_helper.hpp" #include "split.hpp" @@ -104,20 +108,17 @@ ComponentManager::OnLoadNode( auto node_factory = loader->createInstance(clazz); std::vector parameters; - for (auto & p: request->parameters) - { + for (auto & p : request->parameters) { parameters.push_back(rclcpp::Parameter::from_parameter_msg(p)); } - std::vector remap_rules { request->remap_rules }; + std::vector remap_rules {request->remap_rules}; - if (request->node_name.length()) - { + if (request->node_name.length()) { remap_rules.push_back("__node:=" + request->node_name); } - if (request->node_namespace.length()) - { + if (request->node_namespace.length()) { remap_rules.push_back("__ns:=" + request->node_namespace); } @@ -130,8 +131,7 @@ ComponentManager::OnLoadNode( node_wrappers_[node_id] = node_factory->create_node_instance(options); auto node = node_wrappers_[node_id].get_node_base_interface(); - if (auto exec = executor_.lock()) - { + if (auto exec = executor_.lock()) { exec->add_node(node, true); } response->full_node_name = node->get_fully_qualified_name(); @@ -167,18 +167,14 @@ ComponentManager::OnUnloadNode( auto wrapper = node_wrappers_.find(request->unique_id); - if(wrapper == node_wrappers_.end()) - { + 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()) - { + } else { + if (auto exec = executor_.lock()) { exec->remove_node(wrapper->second.get_node_base_interface()); } node_wrappers_.erase(wrapper); @@ -195,11 +191,10 @@ ComponentManager::OnListNodes( (void) request_header; (void) request; - for(auto& wrapper: node_wrappers_) - { + 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()); + wrapper.second.get_node_base_interface()->get_fully_qualified_name()); } } diff --git a/rclcpp_components/src/component_manager.hpp b/rclcpp_components/src/component_manager.hpp index be91601f62..b63eae676c 100644 --- a/rclcpp_components/src/component_manager.hpp +++ b/rclcpp_components/src/component_manager.hpp @@ -12,8 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ -#define RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ +#ifndef COMPONENT_MANAGER_HPP__ +#define COMPONENT_MANAGER_HPP__ + +#include +#include +#include #include "ament_index_cpp/get_resource.hpp" #include "class_loader/class_loader.hpp" @@ -38,7 +42,7 @@ class ComponentManager : public rclcpp::Node using UnloadNode = composition_interfaces::srv::UnloadNode; using ListNodes = composition_interfaces::srv::ListNodes; - ComponentManager(std::weak_ptr executor); + explicit ComponentManager(std::weak_ptr executor); private: void OnLoadNode( @@ -71,4 +75,4 @@ class ComponentManager : public rclcpp::Node } // namespace rclcpp_components -#endif // RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ +#endif // COMPONENT_MANAGER_HPP__ diff --git a/rclcpp_components/src/filesystem_helper.hpp b/rclcpp_components/src/filesystem_helper.hpp index 524b2a2912..54e5cf802e 100644 --- a/rclcpp_components/src/filesystem_helper.hpp +++ b/rclcpp_components/src/filesystem_helper.hpp @@ -1,39 +1,45 @@ -/* - * Copyright (c) 2017, Open Source Robotics Foundation, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2017, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This file is originally from: +// https://github.com/ros/pluginlib/blob/1a4de29fa55173e9b897ca8ff57ebc88c047e0b3/pluginlib/include/pluginlib/impl/filesystem_helper.hpp /// Includes std::filesystem and aliases the namespace to `pluginlib::impl::fs`. /** * If std::filesystem is not available the necessary functions are emulated. */ -#ifndef RCLCPP_COMPONENTS__FILESYSTEM_HELPER_HPP_ -#define RCLCPP_COMPONETNS__FILESYSTEM_HELPER_HPP_ +#ifndef FILESYSTEM_HELPER_HPP_ +#define FILESYSTEM_HELPER_HPP_ #if defined(_MSC_VER) # if _MSC_VER >= 1900 @@ -41,7 +47,7 @@ namespace rclcpp_components { namespace fs = std::experimental::filesystem; -} // namespace components +} // namespace rclcpp_components # define RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM # endif @@ -56,6 +62,7 @@ namespace fs = std::filesystem; } // namespace rclcpp_components # define RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM +// cppcheck-suppress preprocessorErrorDirective # elif __has_include() # include @@ -185,4 +192,4 @@ inline bool exists(const path & path_to_check) #undef RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM -#endif // RCLCPP_COMPONENTS__FILESYSTEM_HELPER_HPP_ +#endif // FILESYSTEM_HELPER_HPP_ diff --git a/rclcpp_components/src/split.hpp b/rclcpp_components/src/split.hpp index 9a0da15a23..cdafb58373 100644 --- a/rclcpp_components/src/split.hpp +++ b/rclcpp_components/src/split.hpp @@ -1,34 +1,40 @@ -/* - * Copyright (c) 2017, Open Source Robotics Foundation, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2017, Open Source Robotics Foundation, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the copyright holders nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This file is originally from: +// https://github.com/ros/pluginlib/blob/1a4de29fa55173e9b897ca8ff57ebc88c047e0b3/pluginlib/include/pluginlib/impl/split.hpp -#ifndef RCLCPP_COMPONENTS__SPLIT_HPP_ -#define RCLCPP_COMPONETNS__SPLIT_HPP_ +#ifndef SPLIT_HPP_ +#define SPLIT_HPP_ #include #include @@ -68,4 +74,4 @@ std::vector split( } // namespace rclcpp_components -#endif // RCLCPP_COMPONENTS__SPLIT_HPP_ +#endif // SPLIT_HPP_ diff --git a/rclcpp_components/test/talker_component.cpp b/rclcpp_components/test/talker_component.cpp deleted file mode 100644 index 909b951462..0000000000 --- a/rclcpp_components/test/talker_component.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using namespace std::chrono_literals; - -namespace test_rclcpp_components -{ -class Talker : public rclcpp::Node -{ -public: - Talker(rclcpp::NodeOptions options) - : Node("talker", options), - count_(0) - { - // Create a publisher of "std_mgs/String" messages on the "chatter" topic. - pub_ = create_publisher("chatter"); - - if(!get_parameter("message", message_)) - { - message_ = "Hello World: "; - } - - // Use a timer to schedule periodic message publishing. - timer_ = create_wall_timer(1s, std::bind(&Talker::on_timer, this)); - } - - void on_timer() - { - auto msg = std::make_shared(); - msg->data = message_ + std::to_string(++count_); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg->data.c_str()); - std::flush(std::cout); - - // Put the message into a queue to be processed by the middleware. - // This call is non-blocking. - pub_->publish(msg); - } - -private: - std::string message_; - size_t count_; - rclcpp::Publisher::SharedPtr pub_; - rclcpp::TimerBase::SharedPtr timer_; -}; -} - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::Talker) From 21f4be4d0a6c0ea03baa6bce1d1cdeee85f8c50d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 27 Mar 2019 13:39:46 -0500 Subject: [PATCH 07/14] Address reviewer feedback. Signed-off-by: Michael Carroll --- .../rclcpp_components/node_factory_template.hpp | 1 + rclcpp_components/rclcpp_components-extras.cmake | 2 +- rclcpp_components/src/component_manager.cpp | 16 ++++++++++------ rclcpp_components/src/component_manager.hpp | 1 - rclcpp_components/src/split.hpp | 2 +- 5 files changed, 13 insertions(+), 9 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp index ef1e94652e..d219ffc721 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ #define RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ +#include #include #include "rclcpp_components/node_factory.hpp" diff --git a/rclcpp_components/rclcpp_components-extras.cmake b/rclcpp_components/rclcpp_components-extras.cmake index 23f0f28934..ce77ab671b 100644 --- a/rclcpp_components/rclcpp_components-extras.cmake +++ b/rclcpp_components/rclcpp_components-extras.cmake @@ -14,7 +14,7 @@ # copied from rclcpp_components/rclcpp_components-extras.cmake -# register ament_package() hook for node plugins once +# 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) diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index b786cadc61..48997e1ce9 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include +#include "ament_index_cpp/get_resource.hpp" + #include "component_manager.hpp" #include "filesystem_helper.hpp" #include "split.hpp" @@ -57,7 +60,7 @@ ComponentManager::OnLoadNode( } std::vector lines = split(content, '\n', true); - for (auto line : lines) { + for (const auto & line : lines) { std::vector parts = split(line, ';'); if (parts.size() != 2) { RCLCPP_ERROR(get_logger(), "Invalid resource entry"); @@ -81,7 +84,7 @@ ComponentManager::OnLoadNode( class_loader::ClassLoader * loader; if (loaders_.find(library_path) != loaders_.end()) { - loader = loaders_[library_path]; + loader = loaders_[library_path].get(); } else { RCLCPP_INFO(get_logger(), "Load library %s", library_path.c_str()); try { @@ -99,26 +102,27 @@ ComponentManager::OnLoadNode( } } auto classes = loader->getAvailableClasses(); - for (auto clazz : classes) { + for (const auto & clazz : classes) { RCLCPP_INFO(get_logger(), "Found class %s", class_name.c_str()); if (clazz == class_name || clazz == fq_class_name) { RCLCPP_INFO(get_logger(), "Instantiate class %s", class_name.c_str()); + loaders_[library_path] = loader; auto node_factory = loader->createInstance(clazz); std::vector parameters; - for (auto & p : request->parameters) { + for (const auto & p : request->parameters) { parameters.push_back(rclcpp::Parameter::from_parameter_msg(p)); } std::vector remap_rules {request->remap_rules}; - if (request->node_name.length()) { + if (!request->node_name.empty()) { remap_rules.push_back("__node:=" + request->node_name); } - if (request->node_namespace.length()) { + if (!request->node_namespace.empty()) { remap_rules.push_back("__ns:=" + request->node_namespace); } diff --git a/rclcpp_components/src/component_manager.hpp b/rclcpp_components/src/component_manager.hpp index b63eae676c..81c0e936f5 100644 --- a/rclcpp_components/src/component_manager.hpp +++ b/rclcpp_components/src/component_manager.hpp @@ -19,7 +19,6 @@ #include #include -#include "ament_index_cpp/get_resource.hpp" #include "class_loader/class_loader.hpp" #include "rclcpp/executor.hpp" diff --git a/rclcpp_components/src/split.hpp b/rclcpp_components/src/split.hpp index cdafb58373..58954cf37c 100644 --- a/rclcpp_components/src/split.hpp +++ b/rclcpp_components/src/split.hpp @@ -64,7 +64,7 @@ std::vector split( ss.str(s); std::string item; while (std::getline(ss, item, delim)) { - if (skip_empty && item == "") { + if (skip_empty && item.empty() ) { continue; } result.push_back(item); From d0dd55c69841926bdc410e9ebd7fc93589c95632 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 27 Mar 2019 15:35:01 -0500 Subject: [PATCH 08/14] Change to smart pointers for managing memory. Signed-off-by: Michael Carroll --- rclcpp_components/src/component_manager.cpp | 12 +++++------- rclcpp_components/src/component_manager.hpp | 8 ++++---- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 48997e1ce9..044fff5b86 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -28,7 +28,8 @@ using namespace std::placeholders; namespace rclcpp_components { -ComponentManager::ComponentManager(std::weak_ptr executor) +ComponentManager::ComponentManager( + std::weak_ptr executor) : Node("ComponentManager"), executor_(executor) { @@ -60,7 +61,7 @@ ComponentManager::OnLoadNode( } std::vector lines = split(content, '\n', true); - for (const auto & line : lines) { + for (const auto & line : lines) { std::vector parts = split(line, ';'); if (parts.size() != 2) { RCLCPP_ERROR(get_logger(), "Invalid resource entry"); @@ -88,7 +89,8 @@ ComponentManager::OnLoadNode( } else { RCLCPP_INFO(get_logger(), "Load library %s", library_path.c_str()); try { - loader = new class_loader::ClassLoader(library_path); + loaders_[library_path] = std::make_unique(library_path); + loader = loaders_[library_path].get(); } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Failed to load library: %s", ex.what()); response->error_message = "Failed to load library"; @@ -106,9 +108,6 @@ ComponentManager::OnLoadNode( RCLCPP_INFO(get_logger(), "Found class %s", class_name.c_str()); if (clazz == class_name || clazz == fq_class_name) { RCLCPP_INFO(get_logger(), "Instantiate class %s", class_name.c_str()); - - loaders_[library_path] = loader; - auto node_factory = loader->createInstance(clazz); std::vector parameters; @@ -146,7 +145,6 @@ ComponentManager::OnLoadNode( } // no matching class found in loader - delete loader; RCLCPP_ERROR( get_logger(), "Failed to find class with the requested plugin name '%s' in " "the loaded library", diff --git a/rclcpp_components/src/component_manager.hpp b/rclcpp_components/src/component_manager.hpp index 81c0e936f5..249f7e0a77 100644 --- a/rclcpp_components/src/component_manager.hpp +++ b/rclcpp_components/src/component_manager.hpp @@ -41,7 +41,8 @@ class ComponentManager : public rclcpp::Node using UnloadNode = composition_interfaces::srv::UnloadNode; using ListNodes = composition_interfaces::srv::ListNodes; - explicit ComponentManager(std::weak_ptr executor); + ComponentManager( + std::weak_ptr executor); private: void OnLoadNode( @@ -62,10 +63,9 @@ class ComponentManager : public rclcpp::Node private: std::weak_ptr executor_; - std::map node_wrappers_; - std::map loaders_; - uint64_t unique_id {1}; + std::map node_wrappers_; + std::map> loaders_; rclcpp::Service::SharedPtr loadNode_srv_; rclcpp::Service::SharedPtr unloadNode_srv_; From 3226e98d24d1877cb68d5e1c68dc7078ace1b59f Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 2 Apr 2019 13:37:37 -0500 Subject: [PATCH 09/14] Update to use rcpputils filesystem/split. Signed-off-by: Michael Carroll --- rclcpp_components/CMakeLists.txt | 21 +- .../rclcpp_components/node_factory.hpp | 2 +- .../node_factory_template.hpp | 2 +- rclcpp_components/package.xml | 1 + .../src/component_container_mt.cpp | 28 +++ rclcpp_components/src/component_manager.cpp | 14 +- rclcpp_components/src/filesystem_helper.hpp | 195 ------------------ rclcpp_components/src/split.hpp | 77 ------- 8 files changed, 57 insertions(+), 283 deletions(-) create mode 100644 rclcpp_components/src/component_container_mt.cpp delete mode 100644 rclcpp_components/src/filesystem_helper.hpp delete mode 100644 rclcpp_components/src/split.hpp diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 9b65f84360..a2ff73fa9c 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -15,13 +15,14 @@ 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_executable( component_container - src/component_manager.cpp src/component_container.cpp + src/component_manager.cpp ) ament_target_dependencies(component_container @@ -29,10 +30,26 @@ ament_target_dependencies(component_container "class_loader" "composition_interfaces" "rclcpp" + "rcpputils" +) + +add_executable( + component_container_mt + src/component_container_mt.cpp + src/component_manager.cpp +) + +ament_target_dependencies(component_container_mt + "ament_index_cpp" + "class_loader" + "composition_interfaces" + "rclcpp" + "rcpputils" ) 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) @@ -42,7 +59,7 @@ endif() # Install executables install( - TARGETS component_container + TARGETS component_container component_container_mt RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp_components/include/rclcpp_components/node_factory.hpp index 0600a482ec..ba8a5c23f0 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -28,7 +28,7 @@ class NodeFactory virtual NodeInstanceWrapper - create_node_instance(rclcpp::NodeOptions options) = 0; + create_node_instance(const rclcpp::NodeOptions & options) = 0; }; } // namespace rclcpp_components diff --git a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp index d219ffc721..af5355f147 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -32,7 +32,7 @@ class NodeFactoryTemplate : public NodeFactory virtual NodeInstanceWrapper - create_node_instance(rclcpp::NodeOptions options) + create_node_instance(const rclcpp::NodeOptions & options) { auto node = std::make_shared(options); diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 00f34b3457..2f18028397 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -13,6 +13,7 @@ class_loader composition_interfaces rclcpp + rcpputils ament_index_cpp class_loader diff --git a/rclcpp_components/src/component_container_mt.cpp b/rclcpp_components/src/component_container_mt.cpp new file mode 100644 index 0000000000..56a0fd1347 --- /dev/null +++ b/rclcpp_components/src/component_container_mt.cpp @@ -0,0 +1,28 @@ +// 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 + +#include "rclcpp/rclcpp.hpp" + +#include "component_manager.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto exec = std::make_shared(); + auto node = std::make_shared(exec); + exec->add_node(node); + exec->spin(); +} diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 044fff5b86..1a1366b727 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "component_manager.hpp" + #include #include #include #include #include "ament_index_cpp/get_resource.hpp" - -#include "component_manager.hpp" -#include "filesystem_helper.hpp" -#include "split.hpp" +#include "rcpputils/filesystem_helper.hpp" +#include "rcpputils/split.hpp" using namespace std::placeholders; @@ -60,9 +60,9 @@ ComponentManager::OnLoadNode( return; } - std::vector lines = split(content, '\n', true); + std::vector lines = rcpputils::split(content, '\n', true); for (const auto & line : lines) { - std::vector parts = split(line, ';'); + std::vector parts = rcpputils::split(line, ';'); if (parts.size() != 2) { RCLCPP_ERROR(get_logger(), "Invalid resource entry"); response->error_message = "Invalid resource entry"; @@ -79,7 +79,7 @@ ComponentManager::OnLoadNode( // load node plugin std::string library_path = parts[1]; - if (!fs::path(library_path).is_absolute()) { + if (!rcpputils::fs::path(library_path).is_absolute()) { library_path = base_path + "/" + library_path; } class_loader::ClassLoader * loader; diff --git a/rclcpp_components/src/filesystem_helper.hpp b/rclcpp_components/src/filesystem_helper.hpp deleted file mode 100644 index 54e5cf802e..0000000000 --- a/rclcpp_components/src/filesystem_helper.hpp +++ /dev/null @@ -1,195 +0,0 @@ -// Copyright (c) 2017, Open Source Robotics Foundation, Inc. -// All rights reserved. -// -// Software License Agreement (BSD License 2.0) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of the copyright holders nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// This file is originally from: -// https://github.com/ros/pluginlib/blob/1a4de29fa55173e9b897ca8ff57ebc88c047e0b3/pluginlib/include/pluginlib/impl/filesystem_helper.hpp - -/// Includes std::filesystem and aliases the namespace to `pluginlib::impl::fs`. -/** - * If std::filesystem is not available the necessary functions are emulated. - */ - -#ifndef FILESYSTEM_HELPER_HPP_ -#define FILESYSTEM_HELPER_HPP_ - -#if defined(_MSC_VER) -# if _MSC_VER >= 1900 -# include -namespace rclcpp_components -{ -namespace fs = std::experimental::filesystem; -} // namespace rclcpp_components - -# define RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM -# endif - -#elif defined(__has_include) -# if __has_include() && __cplusplus >= 201703L -# include - -namespace rclcpp_components -{ -namespace fs = std::filesystem; -} // namespace rclcpp_components - -# define RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM -// cppcheck-suppress preprocessorErrorDirective -# elif __has_include() -# include - -namespace rclcpp_components -{ -namespace fs = std::experimental::filesystem; -} // namespace rclcpp_components - -# define RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM -# endif -#endif - -// The standard library does not provide it, so emulate it. -#ifndef RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM - -#include -#include - -#ifdef _WIN32 -#define CLASS_LOADER_IMPL_OS_DIRSEP '\\' -#else -#define CLASS_LOADER_IMPL_OS_DIRSEP '/' -#endif - -#ifdef _WIN32 - #include - #define access _access_s -#else - #include -#endif - -#include "./split.hpp" - -namespace rclcpp_components -{ -namespace fs -{ - -class path -{ -public: - static constexpr char preferred_separator = CLASS_LOADER_IMPL_OS_DIRSEP; - - path() - : path("") - {} - - path(const std::string & p) // NOLINT(runtime/explicit): this is a conversion constructor - : path_(p), path_as_vector_(split(p, std::string(1, preferred_separator))) - {} - - std::string string() const - { - return path_; - } - - bool exists() const - { - return access(path_.c_str(), 0) == 0; - } - - std::vector::const_iterator cbegin() const - { - return path_as_vector_.cbegin(); - } - - std::vector::const_iterator cend() const - { - return path_as_vector_.cend(); - } - - path parent_path() const - { - path parent(""); - for (auto it = this->cbegin(); it != --this->cend(); ++it) { - parent /= *it; - } - return parent; - } - - path filename() const - { - return path_.empty() ? path() : *--this->cend(); - } - - path operator/(const std::string & other) - { - return this->operator/(path(other)); - } - - path & operator/=(const std::string & other) - { - this->operator/=(path(other)); - return *this; - } - - path operator/(const path & other) - { - return path(*this).operator/=(other); - } - - path & operator/=(const path & other) - { - this->path_ += CLASS_LOADER_IMPL_OS_DIRSEP + other.string(); - this->path_as_vector_.insert( - std::end(this->path_as_vector_), - std::begin(other.path_as_vector_), std::end(other.path_as_vector_)); - return *this; - } - -private: - std::string path_; - std::vector path_as_vector_; -}; - -inline bool exists(const path & path_to_check) -{ - return path_to_check.exists(); -} - -#undef CLASS_LOADER_IMPL_OS_DIRSEP - -} // namespace fs -} // namespace rclcpp_components - -#endif // RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM - -#undef RCLCPP_COMPONENTS__FILESYSYEM_HELPER__HAS_STD_FILESYSTEM - -#endif // FILESYSTEM_HELPER_HPP_ diff --git a/rclcpp_components/src/split.hpp b/rclcpp_components/src/split.hpp deleted file mode 100644 index 58954cf37c..0000000000 --- a/rclcpp_components/src/split.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright (c) 2017, Open Source Robotics Foundation, Inc. -// All rights reserved. -// -// Software License Agreement (BSD License 2.0) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of the copyright holders nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// This file is originally from: -// https://github.com/ros/pluginlib/blob/1a4de29fa55173e9b897ca8ff57ebc88c047e0b3/pluginlib/include/pluginlib/impl/split.hpp - -#ifndef SPLIT_HPP_ -#define SPLIT_HPP_ - -#include -#include -#include - -namespace rclcpp_components -{ - -// Version used in the filesystem helper -inline std::vector -split(const std::string & input, const std::string & regex) -{ - std::regex re(regex); - // the -1 will cause this to return the stuff between the matches, see the submatch argument: - // http://en.cppreference.com/w/cpp/regex/regex_token_iterator/regex_token_iterator - std::sregex_token_iterator first(input.begin(), input.end(), re, -1); - std::sregex_token_iterator last; - return {first, last}; // vector will copy from first to last -} - -// Version used in parsing ament_index -std::vector split( - const std::string & s, char delim, bool skip_empty = false) -{ - std::vector result; - std::stringstream ss; - ss.str(s); - std::string item; - while (std::getline(ss, item, delim)) { - if (skip_empty && item.empty() ) { - continue; - } - result.push_back(item); - } - return result; -} - -} // namespace rclcpp_components - -#endif // SPLIT_HPP_ From 05653d9bd3005d219b9f7aeda82c0db630d6c4ef Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 2 Apr 2019 15:11:14 -0500 Subject: [PATCH 10/14] Address reviewer feedback and add docs. Signed-off-by: Michael Carroll --- .../cmake/rclcpp_components_register_nodes.cmake | 4 ++-- .../include/rclcpp_components/node_factory.hpp | 11 +++++++++++ .../rclcpp_components/node_factory_template.hpp | 13 +++++++++++-- .../rclcpp_components/node_instance_wrapper.hpp | 11 +++++++++++ .../rclcpp_components/register_node_macro.hpp | 12 ++++++++++++ rclcpp_components/package.xml | 2 +- rclcpp_components/src/component_container.cpp | 1 + rclcpp_components/src/component_container_mt.cpp | 1 + 8 files changed, 50 insertions(+), 5 deletions(-) diff --git a/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake b/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake index e8f5ebcc50..eac3841fda 100644 --- a/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake +++ b/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake @@ -13,9 +13,9 @@ # limitations under the License. # -# Register an rclcpp_component with the ament resource index. +# Register an rclcpp component with the ament resource index. # -# The passed library can contain multiple nodes +# The passed library can contain multiple nodes each registered via macro. # # :param target: the shared library target # :type target: string diff --git a/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp_components/include/rclcpp_components/node_factory.hpp index ba8a5c23f0..012240ba38 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -19,6 +19,13 @@ 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 derived constructors to be called when components are loaded. + */ class NodeFactory { public: @@ -26,6 +33,10 @@ class NodeFactory 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; diff --git a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp index af5355f147..e3d02d00af 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -23,6 +23,11 @@ namespace rclcpp_components { +/// NodeFactoryTemplate is a convenience class for instantiating components. +/** + * The NodeFactoryTemplate class can be used to easily proved the NodeFactory interface for + * components that implement a single-argument constructor and `get_node_base_interface`. + */ template class NodeFactoryTemplate : public NodeFactory { @@ -30,14 +35,18 @@ class NodeFactoryTemplate : public NodeFactory NodeFactoryTemplate() = default; virtual ~NodeFactoryTemplate() = 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) { auto node = std::make_shared(options); - return NodeInstanceWrapper(node, - std::bind(&NodeT::get_node_base_interface, node)); + return NodeInstanceWrapper( + node, std::bind(&NodeT::get_node_base_interface, node)); } }; } // namespace rclcpp_components diff --git a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp index 3dbe076942..035c211f57 100644 --- a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp +++ b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp @@ -22,6 +22,7 @@ namespace rclcpp_components { +/// The NodeInstanceWrapper encapsulates the node instance. class NodeInstanceWrapper { public: @@ -38,12 +39,22 @@ class NodeInstanceWrapper : 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 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() { diff --git a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp index 794b250105..e851bb09db 100644 --- a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp +++ b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp @@ -18,6 +18,18 @@ #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, \ rclcpp_components::NodeFactory) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 2f18028397..42fbbd548d 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -3,7 +3,7 @@ rclcpp_components 0.6.2 - Package containing container tools for dynamically loadable components + Package containing tools for dynamically loadable components Michael Carroll Apache License 2.0 diff --git a/rclcpp_components/src/component_container.cpp b/rclcpp_components/src/component_container.cpp index f45fcadb78..e1f3eaf8a4 100644 --- a/rclcpp_components/src/component_container.cpp +++ b/rclcpp_components/src/component_container.cpp @@ -20,6 +20,7 @@ int main(int argc, char * argv[]) { + /// Component container with a single-threaded executor. rclcpp::init(argc, argv); auto exec = std::make_shared(); auto node = std::make_shared(exec); diff --git a/rclcpp_components/src/component_container_mt.cpp b/rclcpp_components/src/component_container_mt.cpp index 56a0fd1347..af8e3a0a84 100644 --- a/rclcpp_components/src/component_container_mt.cpp +++ b/rclcpp_components/src/component_container_mt.cpp @@ -20,6 +20,7 @@ int main(int argc, char * argv[]) { + /// Component container with a multi-threaded executor. rclcpp::init(argc, argv); auto exec = std::make_shared(); auto node = std::make_shared(exec); From 0ce0a5274bbf2fe0bd0f7e6f03cbd8ce00a6b0d5 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 3 Apr 2019 14:07:29 -0500 Subject: [PATCH 11/14] Add tests around ComponentManager. Signed-off-by: Michael Carroll --- rclcpp_components/CMakeLists.txt | 62 +++- rclcpp_components/src/component_manager.cpp | 202 ++++++------ rclcpp_components/src/component_manager.hpp | 34 +- .../test/components/test_component.cpp | 69 ++++ .../components/test_component_manager_api.cpp | 0 .../test/test_component_manager.cpp | 90 ++++++ .../test/test_component_manager_api.cpp | 295 ++++++++++++++++++ 7 files changed, 648 insertions(+), 104 deletions(-) create mode 100644 rclcpp_components/test/components/test_component.cpp create mode 100644 rclcpp_components/test/components/test_component_manager_api.cpp create mode 100644 rclcpp_components/test/test_component_manager.cpp create mode 100644 rclcpp_components/test/test_component_manager_api.cpp diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index a2ff73fa9c..2a50627b5c 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -19,13 +19,12 @@ find_package(rcpputils REQUIRED) include_directories(include) -add_executable( - component_container - src/component_container.cpp +add_library( + component_manager + STATIC src/component_manager.cpp ) - -ament_target_dependencies(component_container +ament_target_dependencies(component_manager "ament_index_cpp" "class_loader" "composition_interfaces" @@ -33,18 +32,22 @@ ament_target_dependencies(component_container "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 - src/component_manager.cpp ) - +target_link_libraries(component_container_mt component_manager) ament_target_dependencies(component_container_mt - "ament_index_cpp" - "class_loader" - "composition_interfaces" "rclcpp" - "rcpputils" ) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") @@ -55,6 +58,43 @@ 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;$\n") + set(components "${components}test_rclcpp_components::TestComponentBar;$\n") + set(components "${components}test_rclcpp_components::TestComponentNoNode;$\n") + + file(GENERATE + OUTPUT + "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$/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}/$") + endif() + + ament_add_gtest(test_component_manager test/test_component_manager.cpp + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$ + 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/$ + 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 diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 1a1366b727..ed846d94b1 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -41,122 +41,146 @@ ComponentManager::ComponentManager( std::bind(&ComponentManager::OnListNodes, this, _1, _2, _3)); } -void -ComponentManager::OnLoadNode( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) +ComponentManager::~ComponentManager() { - (void) request_header; + 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::get_component_resources(const std::string & package_name) const +{ std::string content; std::string base_path; if (!ament_index_cpp::get_resource( - "rclcpp_components", request->package_name, content, &base_path)) + "rclcpp_components", package_name, content, &base_path)) { - RCLCPP_ERROR(get_logger(), "Could not find requested resource in ament index"); - response->error_message = "Could not find requested resource in ament index"; - response->success = false; - return; + throw ComponentManagerException("Could not find requested resource in ament index"); } + std::vector resources; std::vector lines = rcpputils::split(content, '\n', true); for (const auto & line : lines) { std::vector parts = rcpputils::split(line, ';'); if (parts.size() != 2) { - RCLCPP_ERROR(get_logger(), "Invalid resource entry"); - response->error_message = "Invalid resource entry"; - response->success = false; - return; - } - // match plugin name with the same rmw suffix as this executable - if (parts[0] != request->plugin_name) { - continue; + throw ComponentManagerException("Invalid resource entry"); } - std::string class_name = parts[0]; - std::string fq_class_name = "rclcpp_components::NodeFactoryTemplate<" + class_name + ">"; - - // load node plugin std::string library_path = parts[1]; if (!rcpputils::fs::path(library_path).is_absolute()) { library_path = base_path + "/" + library_path; } - class_loader::ClassLoader * loader; - - if (loaders_.find(library_path) != loaders_.end()) { - loader = loaders_[library_path].get(); - } else { - RCLCPP_INFO(get_logger(), "Load library %s", library_path.c_str()); - try { - loaders_[library_path] = std::make_unique(library_path); - loader = loaders_[library_path].get(); - } catch (const std::exception & ex) { - RCLCPP_ERROR(get_logger(), "Failed to load library: %s", ex.what()); - response->error_message = "Failed to load library"; - response->success = false; - return; - } catch (...) { - RCLCPP_ERROR(get_logger(), "Failed to load library"); - response->error_message = "Failed to load library"; - response->success = false; - return; - } + resources.push_back({parts[0], library_path}); + } + return resources; +} + +std::shared_ptr +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(library_path); + } catch (const std::exception & ex) { + throw ComponentManagerException("Failed to load library: " + std::string(ex.what())); + } catch (...) { + throw ComponentManagerException("Failed to load library"); } - auto classes = loader->getAvailableClasses(); - for (const auto & clazz : classes) { - RCLCPP_INFO(get_logger(), "Found class %s", class_name.c_str()); - if (clazz == class_name || clazz == fq_class_name) { - RCLCPP_INFO(get_logger(), "Instantiate class %s", class_name.c_str()); - auto node_factory = loader->createInstance(clazz); - - std::vector parameters; - for (const auto & p : request->parameters) { - parameters.push_back(rclcpp::Parameter::from_parameter_msg(p)); - } - - std::vector 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++; - - node_wrappers_[node_id] = node_factory->create_node_instance(options); - - 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; - } + } + loader = loaders_[library_path].get(); + + auto classes = loader->getAvailableClasses(); + 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(clazz); } + } + return {}; +} - // no matching class found in loader +void +ComponentManager::OnLoadNode( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr 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 parameters; + for (const auto & p : request->parameters) { + parameters.push_back(rclcpp::Parameter::from_parameter_msg(p)); + } + + std::vector 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++; + + auto node_instance = factory->create_node_instance(options); + + node_wrappers_[node_id] = node_instance; + + auto node = node_instance.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; - return; } - - RCLCPP_ERROR( - get_logger(), "Failed to find plugin name '%s' in prefix '%s'", - request->plugin_name.c_str(), base_path.c_str()); - response->success = false; + return; } void diff --git a/rclcpp_components/src/component_manager.hpp b/rclcpp_components/src/component_manager.hpp index 249f7e0a77..fa3d1cc70c 100644 --- a/rclcpp_components/src/component_manager.hpp +++ b/rclcpp_components/src/component_manager.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "class_loader/class_loader.hpp" @@ -34,6 +35,13 @@ 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: @@ -41,21 +49,39 @@ class ComponentManager : public rclcpp::Node 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; + ComponentManager( std::weak_ptr executor); + ~ComponentManager(); + + /// Return a list of valid loadable components in a given package. + std::vector + get_component_resources(const std::string & package_name) const; + + std::shared_ptr + create_component_factory(const ComponentResource & resource); + private: - void OnLoadNode( + void + OnLoadNode( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); - void OnUnloadNode( + void + OnUnloadNode( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); - void OnListNodes( + void + OnListNodes( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); @@ -64,8 +90,8 @@ class ComponentManager : public rclcpp::Node std::weak_ptr executor_; uint64_t unique_id {1}; - std::map node_wrappers_; std::map> loaders_; + std::map node_wrappers_; rclcpp::Service::SharedPtr loadNode_srv_; rclcpp::Service::SharedPtr unloadNode_srv_; diff --git a/rclcpp_components/test/components/test_component.cpp b/rclcpp_components/test/components/test_component.cpp new file mode 100644 index 0000000000..3e1cb251ee --- /dev/null +++ b/rclcpp_components/test/components/test_component.cpp @@ -0,0 +1,69 @@ +// Copyright 2016 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: + TestComponentFoo(rclcpp::NodeOptions options) + : rclcpp::Node("test_component_foo", options) + { + } +}; + +/// Simple test component +class TestComponentBar : public rclcpp::Node +{ +public: + TestComponentBar(rclcpp::NodeOptions options) + : rclcpp::Node("test_component_bar", options) + { + } +}; + +/// Simple test component that doesn't inherit from rclcpp::Node +class TestComponentNoNode +{ +public: + 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) + diff --git a/rclcpp_components/test/components/test_component_manager_api.cpp b/rclcpp_components/test/components/test_component_manager_api.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rclcpp_components/test/test_component_manager.cpp b/rclcpp_components/test/test_component_manager.cpp new file mode 100644 index 0000000000..72e4db892c --- /dev/null +++ b/rclcpp_components/test/test_component_manager.cpp @@ -0,0 +1,90 @@ +// 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 + +#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(); + auto manager = std::make_shared(exec); + + EXPECT_THROW(manager->get_component_resources("invalid_package"), + rclcpp_components::ComponentManagerException); +} + +TEST_F(TestComponentManager, get_component_resources_valid) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(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(); + auto manager = std::make_shared(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(); + auto manager = std::make_shared(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); +} + + diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp new file mode 100644 index 0000000000..0d2669ea96 --- /dev/null +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -0,0 +1,295 @@ +// 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 + +#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(); + auto node = rclcpp::Node::make_shared("test_component_manager"); + auto manager = std::make_shared(exec); + + exec->add_node(manager); + exec->add_node(node); + + auto client = node->create_client( + "/ComponentManager/_container/load_node"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + 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(); + 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(); + 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(); + 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(); + auto node = rclcpp::Node::make_shared("test_component_manager"); + auto manager = std::make_shared(exec); + + exec->add_node(manager); + exec->add_node(node); + + auto client = node->create_client( + "/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(); + 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(); + 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(); + auto node = rclcpp::Node::make_shared("test_component_manager"); + auto manager = std::make_shared(exec); + + exec->add_node(manager); + exec->add_node(node); + + { + auto client = node->create_client( + "/ComponentManager/_container/load_node"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + 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( + "/ComponentManager/_container/list_nodes"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + 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(); + auto node = rclcpp::Node::make_shared("test_component_manager"); + auto manager = std::make_shared(exec); + + exec->add_node(manager); + exec->add_node(node); + + { + auto client = node->create_client( + "/ComponentManager/_container/load_node"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + 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( + "/ComponentManager/_container/unload_node"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + 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(); + 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"); + } + } +} From 2fd16f40c31586055f31185a4d6670a9a776265e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 3 Apr 2019 14:12:59 -0500 Subject: [PATCH 12/14] Lint. Signed-off-by: Michael Carroll --- rclcpp_components/CMakeLists.txt | 4 ++-- rclcpp_components/src/component_manager.cpp | 9 +++----- rclcpp_components/src/component_manager.hpp | 3 ++- .../test/components/test_component.cpp | 15 +++++++------ .../components/test_component_manager_api.cpp | 0 .../test/test_component_manager.cpp | 21 +++++++++---------- .../test/test_component_manager_api.cpp | 3 +++ 7 files changed, 27 insertions(+), 28 deletions(-) delete mode 100644 rclcpp_components/test/components/test_component_manager_api.cpp diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 2a50627b5c..95ba47fa93 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -83,7 +83,7 @@ if(BUILD_TESTING) ament_add_gtest(test_component_manager test/test_component_manager.cpp APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$ APPEND_LIBRARY_DIRS "${append_library_dirs}") - if (TARGET test_component_manager) + if(TARGET test_component_manager) target_link_libraries(test_component_manager component_manager) target_include_directories(test_component_manager PRIVATE src) endif() @@ -91,7 +91,7 @@ if(BUILD_TESTING) 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/$ APPEND_LIBRARY_DIRS "${append_library_dirs}") - if (TARGET test_component_manager_api) + 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() diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index ed846d94b1..88badb44a1 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -43,12 +43,10 @@ ComponentManager::ComponentManager( ComponentManager::~ComponentManager() { - if (node_wrappers_.size()) - { + if (node_wrappers_.size()) { RCLCPP_DEBUG(get_logger(), "Removing components from executor"); if (auto exec = executor_.lock()) { - for (auto & wrapper: node_wrappers_) - { + for (auto & wrapper : node_wrappers_) { exec->remove_node(wrapper.second.get_node_base_interface()); } } @@ -125,7 +123,7 @@ ComponentManager::OnLoadNode( try { auto resources = get_component_resources(request->package_name); - for (const auto & resource: resources) { + for (const auto & resource : resources) { if (resource.first != request->plugin_name) { continue; } @@ -180,7 +178,6 @@ ComponentManager::OnLoadNode( response->error_message = ex.what(); response->success = false; } - return; } void diff --git a/rclcpp_components/src/component_manager.hpp b/rclcpp_components/src/component_manager.hpp index fa3d1cc70c..62d7e1e85d 100644 --- a/rclcpp_components/src/component_manager.hpp +++ b/rclcpp_components/src/component_manager.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "class_loader/class_loader.hpp" @@ -35,7 +36,7 @@ namespace rclcpp_components { -class ComponentManagerException: public std::runtime_error +class ComponentManagerException : public std::runtime_error { public: explicit ComponentManagerException(const std::string & error_desc) diff --git a/rclcpp_components/test/components/test_component.cpp b/rclcpp_components/test/components/test_component.cpp index 3e1cb251ee..dc85187650 100644 --- a/rclcpp_components/test/components/test_component.cpp +++ b/rclcpp_components/test/components/test_component.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. +// 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. @@ -20,8 +20,8 @@ namespace test_rclcpp_components class TestComponentFoo : public rclcpp::Node { public: - TestComponentFoo(rclcpp::NodeOptions options) - : rclcpp::Node("test_component_foo", options) + explicit TestComponentFoo(rclcpp::NodeOptions options) + : rclcpp::Node("test_component_foo", options) { } }; @@ -30,8 +30,8 @@ class TestComponentFoo : public rclcpp::Node class TestComponentBar : public rclcpp::Node { public: - TestComponentBar(rclcpp::NodeOptions options) - : rclcpp::Node("test_component_bar", options) + explicit TestComponentBar(rclcpp::NodeOptions options) + : rclcpp::Node("test_component_bar", options) { } }; @@ -40,8 +40,8 @@ class TestComponentBar : public rclcpp::Node class TestComponentNoNode { public: - TestComponentNoNode(rclcpp::NodeOptions options): - node_("test_component_no_node", options) + explicit TestComponentNoNode(rclcpp::NodeOptions options) + : node_("test_component_no_node", options) { } @@ -66,4 +66,3 @@ class TestComponentNoNode RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentFoo) RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentBar) RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentNoNode) - diff --git a/rclcpp_components/test/components/test_component_manager_api.cpp b/rclcpp_components/test/components/test_component_manager_api.cpp deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/rclcpp_components/test/test_component_manager.cpp b/rclcpp_components/test/test_component_manager.cpp index 72e4db892c..a690111030 100644 --- a/rclcpp_components/test/test_component_manager.cpp +++ b/rclcpp_components/test/test_component_manager.cpp @@ -14,17 +14,19 @@ #include +#include + #include "component_manager.hpp" #include "rcpputils/filesystem_helper.hpp" class TestComponentManager : public ::testing::Test { - protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } }; TEST_F(TestComponentManager, get_component_resources_invalid) @@ -33,7 +35,7 @@ TEST_F(TestComponentManager, get_component_resources_invalid) auto manager = std::make_shared(exec); EXPECT_THROW(manager->get_component_resources("invalid_package"), - rclcpp_components::ComponentManagerException); + rclcpp_components::ComponentManagerException); } TEST_F(TestComponentManager, get_component_resources_valid) @@ -65,8 +67,7 @@ TEST_F(TestComponentManager, create_component_factory_valid) 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) - { + for (const auto & resource : resources) { auto factory = manager->create_component_factory(resource); EXPECT_NE(nullptr, factory); } @@ -79,12 +80,10 @@ TEST_F(TestComponentManager, create_component_factory_invalid) // Test invalid library EXPECT_THROW(manager->create_component_factory({"foo_class", "invalid_library.so"}), - rclcpp_components::ComponentManagerException); + 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); } - - diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index 0d2669ea96..e1db03ad76 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -14,6 +14,9 @@ #include +#include +#include + #include "composition_interfaces/srv/load_node.hpp" #include "composition_interfaces/srv/unload_node.hpp" #include "composition_interfaces/srv/list_nodes.hpp" From d72655dcea61bfd61c33de0ce30c6e0e373b311f Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 3 Apr 2019 14:29:49 -0500 Subject: [PATCH 13/14] Address reviewer feedback and add overflow check. Signed-off-by: Michael Carroll --- .../rclcpp_components/node_factory.hpp | 2 +- .../node_factory_template.hpp | 5 ++--- rclcpp_components/src/component_manager.cpp | 19 ++++++++++++++++++- 3 files changed, 21 insertions(+), 5 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp_components/include/rclcpp_components/node_factory.hpp index 012240ba38..67e6cd7331 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -24,7 +24,7 @@ namespace rclcpp_components /** * The NodeFactory interface serves two purposes: * * It allows for classes not derived from `rclcpp::Node` to be used as components. - * * It derived constructors to be called when components are loaded. + * * It allows derived constructors to be called when components are loaded. */ class NodeFactory { diff --git a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp index e3d02d00af..988b8036bb 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -25,7 +25,7 @@ namespace rclcpp_components /// NodeFactoryTemplate is a convenience class for instantiating components. /** - * The NodeFactoryTemplate class can be used to easily proved the NodeFactory interface for + * 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 @@ -39,9 +39,8 @@ class NodeFactoryTemplate : public NodeFactory /** * \param[in] options Additional options used in the construction of the component. */ - virtual NodeInstanceWrapper - create_node_instance(const rclcpp::NodeOptions & options) + create_node_instance(const rclcpp::NodeOptions & options) override { auto node = std::make_shared(options); diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 88badb44a1..6824777b6f 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -154,7 +154,24 @@ ComponentManager::OnLoadNode( auto node_id = unique_id++; - auto node_instance = factory->create_node_instance(options); + 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 { + auto node_instance = 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"); + } node_wrappers_[node_id] = node_instance; From 4f1a5565e711ad35f24f7cab221361e0d2185b0d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 3 Apr 2019 15:05:04 -0500 Subject: [PATCH 14/14] Fix CI. Signed-off-by: Michael Carroll --- .../include/rclcpp_components/node_instance_wrapper.hpp | 3 ++- rclcpp_components/src/component_manager.cpp | 6 ++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp index 035c211f57..2705e4e8b3 100644 --- a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp +++ b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp @@ -42,7 +42,8 @@ class NodeInstanceWrapper /// 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 + * For most cases `get_node_base_interface` will be sufficient. + * * \return Shared pointer to the encapsulated Node instance. */ const std::shared_ptr diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 6824777b6f..fb258dbe5b 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -166,16 +166,14 @@ ComponentManager::OnLoadNode( } try { - auto node_instance = factory->create_node_instance(options); + 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"); } - node_wrappers_[node_id] = node_instance; - - auto node = node_instance.get_node_base_interface(); + auto node = node_wrappers_[node_id].get_node_base_interface(); if (auto exec = executor_.lock()) { exec->add_node(node, true); }