Skip to content

Commit 853e361

Browse files
author
Christoph Hellmann Santos
committed
Add namespaced controller tests
1 parent 01bf8d7 commit 853e361

File tree

2 files changed

+244
-0
lines changed

2 files changed

+244
-0
lines changed

‎controller_manager/CMakeLists.txt

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -101,6 +101,15 @@ if(BUILD_TESTING)
101101
ros2_control_test_assets::ros2_control_test_assets
102102
)
103103

104+
ament_add_gmock(test_controller_manager_with_namespaced_controllers
105+
test/test_controller_manager_with_namespaced_controllers.cpp
106+
)
107+
target_link_libraries(test_controller_manager_with_namespaced_controllers
108+
controller_manager
109+
test_controller
110+
ros2_control_test_assets::ros2_control_test_assets
111+
)
112+
104113
ament_add_gmock(test_controller_manager_hardware_error_handling
105114
test/test_controller_manager_hardware_error_handling.cpp
106115
)

‎controller_manager/test/test_controller_manager_with_namespaced_controllers.cpp

Lines changed: 235 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,235 @@
1+
// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
// Copyright 2023 Christoph Hellmann Santos
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#include <gtest/gtest.h>
17+
#include <memory>
18+
#include <string>
19+
#include <utility>
20+
#include <vector>
21+
22+
#include "controller_manager/controller_manager.hpp"
23+
#include "controller_manager_msgs/srv/list_controllers.hpp"
24+
#include "controller_manager_test_common.hpp"
25+
#include "lifecycle_msgs/msg/state.hpp"
26+
#include "test_controller/test_controller.hpp"
27+
28+
using ::testing::_;
29+
using ::testing::Return;
30+
31+
class TestControllerManagerWithNamespacedControllers
32+
: public ControllerManagerFixture<controller_manager::ControllerManager>,
33+
public testing::WithParamInterface<Strictness>
34+
{
35+
public:
36+
void SetUp()
37+
{
38+
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
39+
cm_ = std::make_shared<controller_manager::ControllerManager>(
40+
std::make_unique<hardware_interface::ResourceManager>(
41+
ros2_control_test_assets::minimal_robot_urdf, true, true),
42+
executor_, TEST_CM_NAME, "/test_namespace");
43+
run_updater_ = false;
44+
}
45+
};
46+
47+
TEST_P(TestControllerManagerWithNamespacedControllers, controller_in_absolute_namespace)
48+
{
49+
auto test_controller = std::make_shared<test_controller::TestController>();
50+
auto test_controller2 = std::make_shared<test_controller::TestController>();
51+
constexpr char TEST_CONTROLLER1_NAME[] = "/device1/test_controller_name";
52+
constexpr char TEST_CONTROLLER2_NAME[] = "/device2/test_controller_name";
53+
cm_->add_controller(
54+
test_controller, TEST_CONTROLLER1_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
55+
cm_->add_controller(
56+
test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
57+
58+
EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
59+
EXPECT_EQ(2, test_controller.use_count());
60+
61+
// setup interface to claim from controllers
62+
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
63+
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
64+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
65+
{
66+
cmd_itfs_cfg.names.push_back(interface);
67+
}
68+
test_controller->set_command_interface_configuration(cmd_itfs_cfg);
69+
70+
controller_interface::InterfaceConfiguration state_itfs_cfg;
71+
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
72+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
73+
{
74+
state_itfs_cfg.names.push_back(interface);
75+
}
76+
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
77+
{
78+
state_itfs_cfg.names.push_back(interface);
79+
}
80+
test_controller->set_state_interface_configuration(state_itfs_cfg);
81+
82+
controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
83+
cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
84+
for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
85+
{
86+
cmd_itfs_cfg2.names.push_back(interface);
87+
}
88+
test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);
89+
90+
controller_interface::InterfaceConfiguration state_itfs_cfg2;
91+
state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
92+
test_controller2->set_state_interface_configuration(state_itfs_cfg2);
93+
94+
// Check if namespace is set correctly
95+
RCLCPP_INFO(
96+
rclcpp::get_logger("test_controll_manager_namespace"), "Controller Manager namespace is '%s'",
97+
cm_->get_namespace());
98+
EXPECT_STREQ(cm_->get_namespace(), "/test_namespace");
99+
RCLCPP_INFO(
100+
rclcpp::get_logger("test_controll_manager_namespace"), "Controller 1 namespace is '%s'",
101+
test_controller->get_node()->get_namespace());
102+
EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/device1");
103+
RCLCPP_INFO(
104+
rclcpp::get_logger("test_controll_manager_namespace"), "Controller 2 namespace is '%s'",
105+
test_controller2->get_node()->get_namespace());
106+
EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/device2");
107+
}
108+
109+
TEST_P(TestControllerManagerWithNamespacedControllers, controller_in_same_namespace)
110+
{
111+
auto test_controller = std::make_shared<test_controller::TestController>();
112+
auto test_controller2 = std::make_shared<test_controller::TestController>();
113+
constexpr char TEST_CONTROLLER1_NAME[] = "test_controller1_name";
114+
constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
115+
cm_->add_controller(
116+
test_controller, TEST_CONTROLLER1_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
117+
cm_->add_controller(
118+
test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
119+
120+
EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
121+
EXPECT_EQ(2, test_controller.use_count());
122+
123+
// setup interface to claim from controllers
124+
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
125+
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
126+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
127+
{
128+
cmd_itfs_cfg.names.push_back(interface);
129+
}
130+
test_controller->set_command_interface_configuration(cmd_itfs_cfg);
131+
132+
controller_interface::InterfaceConfiguration state_itfs_cfg;
133+
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
134+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
135+
{
136+
state_itfs_cfg.names.push_back(interface);
137+
}
138+
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
139+
{
140+
state_itfs_cfg.names.push_back(interface);
141+
}
142+
test_controller->set_state_interface_configuration(state_itfs_cfg);
143+
144+
controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
145+
cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
146+
for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
147+
{
148+
cmd_itfs_cfg2.names.push_back(interface);
149+
}
150+
test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);
151+
152+
controller_interface::InterfaceConfiguration state_itfs_cfg2;
153+
state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
154+
test_controller2->set_state_interface_configuration(state_itfs_cfg2);
155+
156+
// Check if namespace is set correctly
157+
RCLCPP_INFO(
158+
rclcpp::get_logger("test_controll_manager_namespace"), "Controller Manager namespace is '%s'",
159+
cm_->get_namespace());
160+
EXPECT_STREQ(cm_->get_namespace(), "/test_namespace");
161+
RCLCPP_INFO(
162+
rclcpp::get_logger("test_controll_manager_namespace"), "Controller 1 namespace is '%s'",
163+
test_controller->get_node()->get_namespace());
164+
EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/test_namespace");
165+
RCLCPP_INFO(
166+
rclcpp::get_logger("test_controll_manager_namespace"), "Controller 2 namespace is '%s'",
167+
test_controller2->get_node()->get_namespace());
168+
EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/test_namespace");
169+
}
170+
171+
TEST_P(TestControllerManagerWithNamespacedControllers, controller_in_relative_namespace)
172+
{
173+
auto test_controller = std::make_shared<test_controller::TestController>();
174+
auto test_controller2 = std::make_shared<test_controller::TestController>();
175+
constexpr char TEST_CONTROLLER1_NAME[] = "device1/test_controller1_name";
176+
constexpr char TEST_CONTROLLER2_NAME[] = "device2/test_controller2_name";
177+
cm_->add_controller(
178+
test_controller, TEST_CONTROLLER1_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
179+
cm_->add_controller(
180+
test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
181+
182+
EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
183+
EXPECT_EQ(2, test_controller.use_count());
184+
185+
// setup interface to claim from controllers
186+
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
187+
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
188+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
189+
{
190+
cmd_itfs_cfg.names.push_back(interface);
191+
}
192+
test_controller->set_command_interface_configuration(cmd_itfs_cfg);
193+
194+
controller_interface::InterfaceConfiguration state_itfs_cfg;
195+
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
196+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
197+
{
198+
state_itfs_cfg.names.push_back(interface);
199+
}
200+
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
201+
{
202+
state_itfs_cfg.names.push_back(interface);
203+
}
204+
test_controller->set_state_interface_configuration(state_itfs_cfg);
205+
206+
controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
207+
cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
208+
for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
209+
{
210+
cmd_itfs_cfg2.names.push_back(interface);
211+
}
212+
test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);
213+
214+
controller_interface::InterfaceConfiguration state_itfs_cfg2;
215+
state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
216+
test_controller2->set_state_interface_configuration(state_itfs_cfg2);
217+
218+
// Check if namespace is set correctly
219+
RCLCPP_INFO(
220+
rclcpp::get_logger("test_controll_manager_namespace"), "Controller Manager namespace is '%s'",
221+
cm_->get_namespace());
222+
EXPECT_STREQ(cm_->get_namespace(), "/test_namespace");
223+
RCLCPP_INFO(
224+
rclcpp::get_logger("test_controll_manager_namespace"), "Controller 1 namespace is '%s'",
225+
test_controller->get_node()->get_namespace());
226+
EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/test_namespace/device1");
227+
RCLCPP_INFO(
228+
rclcpp::get_logger("test_controll_manager_namespace"), "Controller 2 namespace is '%s'",
229+
test_controller2->get_node()->get_namespace());
230+
EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/test_namespace/device2");
231+
}
232+
233+
INSTANTIATE_TEST_SUITE_P(
234+
test_strict_best_effort, TestControllerManagerWithNamespacedControllers,
235+
testing::Values(strict, best_effort));

0 commit comments

Comments
 (0)