Skip to content

Commit 321c74c

Browse files
gezpgezp
authored
create component_container_isolated (#1781)
Signed-off-by: zhenpeng ge <[email protected]>
1 parent 536df11 commit 321c74c

6 files changed

Lines changed: 218 additions & 10 deletions

File tree

‎rclcpp_components/CMakeLists.txt‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -61,9 +61,20 @@ ament_target_dependencies(component_container_mt
6161
"rclcpp"
6262
)
6363

64+
add_executable(
65+
component_container_isolated
66+
src/component_container_isolated.cpp
67+
)
68+
target_link_libraries(component_container_isolated component_manager)
69+
ament_target_dependencies(component_container_isolated
70+
"rclcpp"
71+
)
72+
73+
6474
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
6575
target_link_libraries(component_container "stdc++fs")
6676
target_link_libraries(component_container_mt "stdc++fs")
77+
target_link_libraries(component_container_isolated "stdc++fs")
6778
endif()
6879

6980
if(BUILD_TESTING)
@@ -135,7 +146,7 @@ install(
135146

136147
# Install executables
137148
install(
138-
TARGETS component_container component_container_mt
149+
TARGETS component_container component_container_mt component_container_isolated
139150
RUNTIME DESTINATION lib/${PROJECT_NAME}
140151
)
141152

‎rclcpp_components/include/rclcpp_components/component_manager.hpp‎

Lines changed: 17 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -140,6 +140,22 @@ class ComponentManager : public rclcpp::Node
140140
virtual rclcpp::NodeOptions
141141
create_node_options(const std::shared_ptr<LoadNode::Request> request);
142142

143+
/// Add component node to executor model, it's invoked in on_load_node()
144+
/**
145+
* \param node_id node_id of loaded component node in node_wrappers_
146+
*/
147+
RCLCPP_COMPONENTS_PUBLIC
148+
virtual void
149+
add_node_to_executor(uint64_t node_id);
150+
151+
/// Remove component node from executor model, it's invoked in on_unload_node()
152+
/**
153+
* \param node_id node_id of loaded component node in node_wrappers_
154+
*/
155+
RCLCPP_COMPONENTS_PUBLIC
156+
virtual void
157+
remove_node_from_executor(uint64_t node_id);
158+
143159
/// Service callback to load a new node in the component
144160
/**
145161
* This function allows to add parameters, remap rules, a specific node, name a namespace
@@ -231,7 +247,7 @@ class ComponentManager : public rclcpp::Node
231247
on_list_nodes(request_header, request, response);
232248
}
233249

234-
private:
250+
protected:
235251
std::weak_ptr<rclcpp::Executor> executor_;
236252

237253
uint64_t unique_id_ {1};

‎rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
// Copyright 2021 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
16+
#ifndef RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__
17+
#define RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__
18+
19+
#include <map>
20+
#include <memory>
21+
#include <string>
22+
#include <utility>
23+
#include <vector>
24+
#include <unordered_map>
25+
26+
#include "rclcpp_components/component_manager.hpp"
27+
28+
29+
namespace rclcpp_components
30+
{
31+
/// ComponentManagerIsolated uses dedicated single-threaded executors for each components.
32+
template<typename ExecutorT = rclcpp::executors::SingleThreadedExecutor>
33+
class ComponentManagerIsolated : public rclcpp_components::ComponentManager
34+
{
35+
using rclcpp_components::ComponentManager::ComponentManager;
36+
37+
struct DedicatedExecutorWrapper
38+
{
39+
std::shared_ptr<rclcpp::Executor> executor;
40+
std::thread thread;
41+
std::promise<void> promise;
42+
};
43+
44+
public:
45+
~ComponentManagerIsolated()
46+
{
47+
if (node_wrappers_.size()) {
48+
for (auto & executor_wrapper : dedicated_executor_wrappers_) {
49+
executor_wrapper.second.promise.set_value();
50+
executor_wrapper.second.executor->cancel();
51+
executor_wrapper.second.thread.join();
52+
}
53+
node_wrappers_.clear();
54+
}
55+
}
56+
57+
protected:
58+
/// Add component node to executor model, it's invoked in on_load_node()
59+
/**
60+
* \param node_id node_id of loaded component node in node_wrappers_
61+
*/
62+
RCLCPP_COMPONENTS_PUBLIC
63+
void
64+
add_node_to_executor(uint64_t node_id) override
65+
{
66+
DedicatedExecutorWrapper executor_wrapper;
67+
auto exec = std::make_shared<ExecutorT>();
68+
exec->add_node(node_wrappers_[node_id].get_node_base_interface());
69+
executor_wrapper.executor = exec;
70+
executor_wrapper.thread = std::thread(
71+
[exec, cancel_token = executor_wrapper.promise.get_future()]() {
72+
exec->spin_until_future_complete(cancel_token);
73+
});
74+
dedicated_executor_wrappers_[node_id] = std::move(executor_wrapper);
75+
}
76+
/// Remove component node from executor model, it's invoked in on_unload_node()
77+
/**
78+
* \param node_id node_id of loaded component node in node_wrappers_
79+
*/
80+
RCLCPP_COMPONENTS_PUBLIC
81+
void
82+
remove_node_from_executor(uint64_t node_id) override
83+
{
84+
auto executor_wrapper = dedicated_executor_wrappers_.find(node_id);
85+
if (executor_wrapper != dedicated_executor_wrappers_.end()) {
86+
executor_wrapper->second.promise.set_value();
87+
executor_wrapper->second.executor->cancel();
88+
executor_wrapper->second.thread.join();
89+
dedicated_executor_wrappers_.erase(executor_wrapper);
90+
}
91+
}
92+
93+
private:
94+
std::unordered_map<uint64_t, DedicatedExecutorWrapper> dedicated_executor_wrappers_;
95+
};
96+
97+
} // namespace rclcpp_components
98+
99+
#endif // RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__

‎rclcpp_components/src/component_container_isolated.cpp‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
// Copyright 2021 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <memory>
16+
#include <vector>
17+
#include <string>
18+
19+
#include "rclcpp/rclcpp.hpp"
20+
#include "rclcpp/utilities.hpp"
21+
#include "rclcpp_components/component_manager_isolated.hpp"
22+
23+
int main(int argc, char * argv[])
24+
{
25+
/// Component container with dedicated single-threaded executors for each components.
26+
rclcpp::init(argc, argv);
27+
// parse arguments
28+
bool use_multi_threaded_executor{false};
29+
std::vector<std::string> args = rclcpp::remove_ros_arguments(argc, argv);
30+
for (auto & arg : args) {
31+
if (arg == std::string("--use_multi_threaded_executor")) {
32+
use_multi_threaded_executor = true;
33+
}
34+
}
35+
// create executor and component manager
36+
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
37+
rclcpp::Node::SharedPtr node;
38+
if (use_multi_threaded_executor) {
39+
using ComponentManagerIsolated =
40+
rclcpp_components::ComponentManagerIsolated<rclcpp::executors::MultiThreadedExecutor>;
41+
node = std::make_shared<ComponentManagerIsolated>(exec);
42+
} else {
43+
using ComponentManagerIsolated =
44+
rclcpp_components::ComponentManagerIsolated<rclcpp::executors::SingleThreadedExecutor>;
45+
node = std::make_shared<ComponentManagerIsolated>(exec);
46+
}
47+
exec->add_node(node);
48+
exec->spin();
49+
}

‎rclcpp_components/src/component_manager.cpp‎

Lines changed: 19 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,22 @@ ComponentManager::create_node_options(const std::shared_ptr<LoadNode::Request> r
166166
return options;
167167
}
168168

169+
void
170+
ComponentManager::add_node_to_executor(uint64_t node_id)
171+
{
172+
if (auto exec = executor_.lock()) {
173+
exec->add_node(node_wrappers_[node_id].get_node_base_interface(), true);
174+
}
175+
}
176+
177+
void
178+
ComponentManager::remove_node_from_executor(uint64_t node_id)
179+
{
180+
if (auto exec = executor_.lock()) {
181+
exec->remove_node(node_wrappers_[node_id].get_node_base_interface());
182+
}
183+
}
184+
169185
void
170186
ComponentManager::on_load_node(
171187
const std::shared_ptr<rmw_request_id_t> request_header,
@@ -214,10 +230,9 @@ ComponentManager::on_load_node(
214230
throw ComponentManagerException("Component constructor threw an exception");
215231
}
216232

233+
add_node_to_executor(node_id);
234+
217235
auto node = node_wrappers_[node_id].get_node_base_interface();
218-
if (auto exec = executor_.lock()) {
219-
exec->add_node(node, true);
220-
}
221236
response->full_node_name = node->get_fully_qualified_name();
222237
response->unique_id = node_id;
223238
response->success = true;
@@ -253,9 +268,7 @@ ComponentManager::on_unload_node(
253268
response->error_message = ss.str();
254269
RCLCPP_WARN(get_logger(), "%s", ss.str().c_str());
255270
} else {
256-
if (auto exec = executor_.lock()) {
257-
exec->remove_node(wrapper->second.get_node_base_interface());
258-
}
271+
remove_node_from_executor(request->unique_id);
259272
node_wrappers_.erase(wrapper);
260273
response->success = true;
261274
}

‎rclcpp_components/test/test_component_manager_api.cpp‎

Lines changed: 22 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include "composition_interfaces/srv/list_nodes.hpp"
2323

2424
#include "rclcpp_components/component_manager.hpp"
25+
#include "rclcpp_components/component_manager_isolated.hpp"
2526

2627
using namespace std::chrono_literals;
2728

@@ -36,11 +37,18 @@ class TestComponentManager : public ::testing::Test
3637

3738
// TODO(hidmic): split up tests once Node bring up/tear down races
3839
// are solved https://github.com/ros2/rclcpp/issues/863
39-
TEST_F(TestComponentManager, components_api)
40+
void test_components_api(bool use_dedicated_executor)
4041
{
4142
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
4243
auto node = rclcpp::Node::make_shared("test_component_manager");
43-
auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
44+
std::shared_ptr<rclcpp_components::ComponentManager> manager;
45+
if (use_dedicated_executor) {
46+
using ComponentManagerIsolated =
47+
rclcpp_components::ComponentManagerIsolated<rclcpp::executors::SingleThreadedExecutor>;
48+
manager = std::make_shared<ComponentManagerIsolated>(exec);
49+
} else {
50+
manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
51+
}
4452

4553
exec->add_node(manager);
4654
exec->add_node(node);
@@ -321,3 +329,15 @@ TEST_F(TestComponentManager, components_api)
321329
}
322330
}
323331
}
332+
333+
TEST_F(TestComponentManager, components_api)
334+
{
335+
{
336+
SCOPED_TRACE("ComponentManager");
337+
test_components_api(false);
338+
}
339+
{
340+
SCOPED_TRACE("ComponentManagerIsolated");
341+
test_components_api(true);
342+
}
343+
}

0 commit comments

Comments
 (0)