Introduce rclcpp_components to implement composition (#665)

* Introduce rclcpp_components package Signed-off-by: Michael Carroll <michael@openrobotics.org> * Keep pointer to NodeWrapper vs NodeInterface. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Remove component registration from rclcpp Signed-off-by: Michael Carroll <michael@openrobotics.org> * Make topics names private-prefix. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Handle name and namespace with remap rules. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Linting. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Address reviewer feedback. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Change to smart pointers for managing memory. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Update to use rcpputils filesystem/split. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Address reviewer feedback and add docs. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Add tests around ComponentManager. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Lint. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Address reviewer feedback and add overflow check. Signed-off-by: Michael Carroll <michael@openrobotics.org> * Fix CI. Signed-off-by: Michael Carroll <michael@openrobotics.org>
2019-04-04 11:16:32 -05:00 · 2019-04-04 11:16:32 -05:00 · 0f25f714fe
commit 0f25f714fe
parent d11a10a583
18 changed files with 1243 additions and 56 deletions
--- a/rclcpp_components/test/components/test_component.cpp
+++ b/rclcpp_components/test/components/test_component.cpp
@ -0,0 +1,68 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace test_rclcpp_components
+{
+/// Simple test component
+class TestComponentFoo : public rclcpp::Node
+{
+public:
+  explicit TestComponentFoo(rclcpp::NodeOptions options)
+  : rclcpp::Node("test_component_foo", options)
+  {
+  }
+};
+
+/// Simple test component
+class TestComponentBar : public rclcpp::Node
+{
+public:
+  explicit TestComponentBar(rclcpp::NodeOptions options)
+  : rclcpp::Node("test_component_bar", options)
+  {
+  }
+};
+
+/// Simple test component that doesn't inherit from rclcpp::Node
+class TestComponentNoNode
+{
+public:
+  explicit TestComponentNoNode(rclcpp::NodeOptions options)
+  : node_("test_component_no_node", options)
+  {
+  }
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
+  get_node_base_interface()
+  {
+    return node_.get_node_base_interface();
+  }
+
+private:
+  rclcpp::Node node_;
+};
+
+
+}  // namespace test_rclcpp_components
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentFoo)
+RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentBar)
+RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentNoNode)
--- a/rclcpp_components/test/test_component_manager.cpp
+++ b/rclcpp_components/test/test_component_manager.cpp
@ -0,0 +1,89 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+#include "component_manager.hpp"
+
+#include "rcpputils/filesystem_helper.hpp"
+
+class TestComponentManager : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+TEST_F(TestComponentManager, get_component_resources_invalid)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  EXPECT_THROW(manager->get_component_resources("invalid_package"),
+    rclcpp_components::ComponentManagerException);
+}
+
+TEST_F(TestComponentManager, get_component_resources_valid)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  auto resources = manager->get_component_resources("rclcpp_components");
+  EXPECT_EQ(3u, resources.size());
+
+  EXPECT_EQ("test_rclcpp_components::TestComponentFoo", resources[0].first);
+  EXPECT_EQ("test_rclcpp_components::TestComponentBar", resources[1].first);
+  EXPECT_EQ("test_rclcpp_components::TestComponentNoNode", resources[2].first);
+
+  EXPECT_TRUE(rcpputils::fs::path(resources[0].second).exists());
+  EXPECT_TRUE(rcpputils::fs::path(resources[1].second).exists());
+  EXPECT_TRUE(rcpputils::fs::path(resources[2].second).exists());
+}
+
+TEST_F(TestComponentManager, create_component_factory_valid)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  auto resources = manager->get_component_resources("rclcpp_components");
+  EXPECT_EQ(3u, resources.size());
+
+  // Repeated loading should reuse existing class loader and not throw.
+  EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]););
+  EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]););
+
+  for (const auto & resource : resources) {
+    auto factory = manager->create_component_factory(resource);
+    EXPECT_NE(nullptr, factory);
+  }
+}
+
+TEST_F(TestComponentManager, create_component_factory_invalid)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  // Test invalid library
+  EXPECT_THROW(manager->create_component_factory({"foo_class", "invalid_library.so"}),
+    rclcpp_components::ComponentManagerException);
+
+  // Test valid library with invalid class
+  auto resources = manager->get_component_resources("rclcpp_components");
+  auto factory = manager->create_component_factory({"foo_class", resources[0].second});
+  EXPECT_EQ(nullptr, factory);
+}
--- a/rclcpp_components/test/test_component_manager_api.cpp
+++ b/rclcpp_components/test/test_component_manager_api.cpp
@ -0,0 +1,298 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <string>
+
+#include "composition_interfaces/srv/load_node.hpp"
+#include "composition_interfaces/srv/unload_node.hpp"
+#include "composition_interfaces/srv/list_nodes.hpp"
+
+#include "component_manager.hpp"
+
+using namespace std::chrono_literals;
+
+class TestComponentManager : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+TEST_F(TestComponentManager, load_components)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto node = rclcpp::Node::make_shared("test_component_manager");
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  exec->add_node(manager);
+  exec->add_node(node);
+
+  auto client = node->create_client<composition_interfaces::srv::LoadNode>(
+    "/ComponentManager/_container/load_node");
+
+  if (!client->wait_for_service(20s)) {
+    ASSERT_TRUE(false) << "service not available after waiting";
+  }
+
+  {
+    auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+    request->package_name = "rclcpp_components";
+    request->plugin_name = "test_rclcpp_components::TestComponentFoo";
+
+    auto result = client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+    EXPECT_EQ(result.get()->success, true);
+    EXPECT_EQ(result.get()->error_message, "");
+    EXPECT_EQ(result.get()->full_node_name, "/test_component_foo");
+    EXPECT_EQ(result.get()->unique_id, 1u);
+  }
+
+  {
+    auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+    request->package_name = "rclcpp_components";
+    request->plugin_name = "test_rclcpp_components::TestComponentBar";
+
+    auto result = client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+    EXPECT_EQ(result.get()->success, true);
+    EXPECT_EQ(result.get()->error_message, "");
+    EXPECT_EQ(result.get()->full_node_name, "/test_component_bar");
+    EXPECT_EQ(result.get()->unique_id, 2u);
+  }
+
+  // Test remapping the node name
+  {
+    auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+    request->package_name = "rclcpp_components";
+    request->plugin_name = "test_rclcpp_components::TestComponentFoo";
+    request->node_name = "test_component_baz";
+
+    auto result = client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+    EXPECT_EQ(result.get()->success, true);
+    EXPECT_EQ(result.get()->error_message, "");
+    EXPECT_EQ(result.get()->full_node_name, "/test_component_baz");
+    EXPECT_EQ(result.get()->unique_id, 3u);
+  }
+
+  // Test remapping the node namespace
+  {
+    auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+    request->package_name = "rclcpp_components";
+    request->plugin_name = "test_rclcpp_components::TestComponentFoo";
+    request->node_namespace = "/ns";
+    request->node_name = "test_component_bing";
+
+    auto result = client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+    EXPECT_EQ(result.get()->success, true);
+    EXPECT_EQ(result.get()->error_message, "");
+    EXPECT_EQ(result.get()->full_node_name, "/ns/test_component_bing");
+    EXPECT_EQ(result.get()->unique_id, 4u);
+  }
+
+  auto node_names = node->get_node_names();
+
+  auto find_in_nodes = [node_names](std::string name) {
+      return std::find(node_names.begin(), node_names.end(), name) != node_names.end();
+    };
+
+  EXPECT_TRUE(find_in_nodes("test_component_foo"));
+  EXPECT_TRUE(find_in_nodes("test_component_bar"));
+  EXPECT_TRUE(find_in_nodes("test_component_baz"));
+  EXPECT_TRUE(find_in_nodes("test_component_bing"));
+}
+
+TEST_F(TestComponentManager, load_invalid_components)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto node = rclcpp::Node::make_shared("test_component_manager");
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  exec->add_node(manager);
+  exec->add_node(node);
+
+  auto client = node->create_client<composition_interfaces::srv::LoadNode>(
+    "/ComponentManager/_container/load_node");
+
+  if (!client->wait_for_service(20s)) {
+    ASSERT_TRUE(false) << "service not available after waiting";
+  }
+
+  {
+    // Valid package, but invalid class name.
+    auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+    request->package_name = "rclcpp_components";
+    request->plugin_name = "test_rclcpp_components::TestComponent";
+
+    auto result = client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+    EXPECT_EQ(result.get()->success, false);
+    EXPECT_EQ(result.get()->error_message, "Failed to find class with the requested plugin name.");
+    EXPECT_EQ(result.get()->full_node_name, "");
+    EXPECT_EQ(result.get()->unique_id, 0u);
+  }
+
+  {
+    // Invalid package, but valid class name.
+    auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+    request->package_name = "rclcpp_components_foo";
+    request->plugin_name = "test_rclcpp_components::TestComponentFoo";
+
+    auto result = client->async_send_request(request);
+    auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+    EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+    EXPECT_EQ(result.get()->success, false);
+    EXPECT_EQ(result.get()->error_message, "Could not find requested resource in ament index");
+    EXPECT_EQ(result.get()->full_node_name, "");
+    EXPECT_EQ(result.get()->unique_id, 0u);
+  }
+}
+
+
+TEST_F(TestComponentManager, list_components)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto node = rclcpp::Node::make_shared("test_component_manager");
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  exec->add_node(manager);
+  exec->add_node(node);
+
+  {
+    auto client = node->create_client<composition_interfaces::srv::LoadNode>(
+      "/ComponentManager/_container/load_node");
+
+    if (!client->wait_for_service(20s)) {
+      ASSERT_TRUE(false) << "service not available after waiting";
+    }
+
+    {
+      auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+      request->package_name = "rclcpp_components";
+      request->plugin_name = "test_rclcpp_components::TestComponentFoo";
+
+      auto result = client->async_send_request(request);
+      auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+      EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+      EXPECT_EQ(result.get()->success, true);
+      EXPECT_EQ(result.get()->error_message, "");
+      EXPECT_EQ(result.get()->full_node_name, "/test_component_foo");
+      EXPECT_EQ(result.get()->unique_id, 1u);
+    }
+  }
+
+  {
+    auto client = node->create_client<composition_interfaces::srv::ListNodes>(
+      "/ComponentManager/_container/list_nodes");
+
+    if (!client->wait_for_service(20s)) {
+      ASSERT_TRUE(false) << "service not available after waiting";
+    }
+
+    {
+      auto request = std::make_shared<composition_interfaces::srv::ListNodes::Request>();
+      auto result = client->async_send_request(request);
+      auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+      EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+      auto node_names = result.get()->full_node_names;
+      auto unique_ids = result.get()->unique_ids;
+
+      EXPECT_EQ(node_names.size(), 1u);
+      EXPECT_EQ(node_names[0], "/test_component_foo");
+      EXPECT_EQ(unique_ids.size(), 1u);
+      EXPECT_EQ(unique_ids[0], 1u);
+    }
+  }
+}
+
+TEST_F(TestComponentManager, unload_component)
+{
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  auto node = rclcpp::Node::make_shared("test_component_manager");
+  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+
+  exec->add_node(manager);
+  exec->add_node(node);
+
+  {
+    auto client = node->create_client<composition_interfaces::srv::LoadNode>(
+      "/ComponentManager/_container/load_node");
+
+    if (!client->wait_for_service(20s)) {
+      ASSERT_TRUE(false) << "service not available after waiting";
+    }
+
+    {
+      auto request = std::make_shared<composition_interfaces::srv::LoadNode::Request>();
+      request->package_name = "rclcpp_components";
+      request->plugin_name = "test_rclcpp_components::TestComponentFoo";
+
+      auto result = client->async_send_request(request);
+      auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+      EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+      EXPECT_EQ(result.get()->success, true);
+      EXPECT_EQ(result.get()->error_message, "");
+      EXPECT_EQ(result.get()->full_node_name, "/test_component_foo");
+      EXPECT_EQ(result.get()->unique_id, 1u);
+    }
+  }
+
+  auto node_names = node->get_node_names();
+  auto find_in_nodes = [node_names](std::string name) {
+      return std::find(node_names.begin(), node_names.end(), name) != node_names.end();
+    };
+  EXPECT_TRUE(find_in_nodes("test_component_foo"));
+
+  {
+    auto client = node->create_client<composition_interfaces::srv::UnloadNode>(
+      "/ComponentManager/_container/unload_node");
+
+    if (!client->wait_for_service(20s)) {
+      ASSERT_TRUE(false) << "service not available after waiting";
+    }
+
+    {
+      auto request = std::make_shared<composition_interfaces::srv::UnloadNode::Request>();
+      request->unique_id = 1;
+
+      auto result = client->async_send_request(request);
+      auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+      EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+      EXPECT_EQ(result.get()->success, true);
+      EXPECT_EQ(result.get()->error_message, "");
+    }
+
+    {
+      auto request = std::make_shared<composition_interfaces::srv::UnloadNode::Request>();
+      request->unique_id = 1;
+
+      auto result = client->async_send_request(request);
+      auto ret = exec->spin_until_future_complete(result, 5s);  // Wait for the result.
+      EXPECT_EQ(ret, rclcpp::executor::FutureReturnCode::SUCCESS);
+      EXPECT_EQ(result.get()->success, false);
+      EXPECT_EQ(result.get()->error_message, "No node found with unique_id: 1");
+    }
+  }
+}