Skip to content

Commit 848c8ae

Browse files
saikishorsaikishor
committed
Added test to reproduce the failure of switching with exclusive interfaces
1 parent 6a29b34 commit 848c8ae

File tree

2 files changed

+133
-0
lines changed

2 files changed

+133
-0
lines changed

‎controller_manager/test/test_release_interfaces.cpp

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include "controller_manager/controller_manager.hpp"
2323
#include "controller_manager_test_common.hpp"
2424
#include "lifecycle_msgs/msg/state.hpp"
25+
#include "test_controller_failed_activate/test_controller_failed_activate.hpp"
2526
#include "test_controller_with_interfaces/test_controller_with_interfaces.hpp"
2627

2728
using ::testing::_;
@@ -31,6 +32,19 @@ class TestReleaseInterfaces : public ControllerManagerFixture<controller_manager
3132
{
3233
};
3334

35+
class TestReleaseExclusiveInterfaces
36+
: public ControllerManagerFixture<controller_manager::ControllerManager>
37+
{
38+
public:
39+
TestReleaseExclusiveInterfaces()
40+
: ControllerManagerFixture<controller_manager::ControllerManager>(
41+
std::string(ros2_control_test_assets::urdf_head) +
42+
std::string(ros2_control_test_assets::hardware_resources_with_exclusive_interface) +
43+
std::string(ros2_control_test_assets::urdf_tail))
44+
{
45+
}
46+
};
47+
3448
TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
3549
{
3650
std::string controller_type =
@@ -199,3 +213,69 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
199213
abstract_test_controller2.c->get_state().id());
200214
}
201215
}
216+
217+
TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failure)
218+
{
219+
std::string controller_type =
220+
test_controller_failed_activate::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME;
221+
222+
// Load two controllers of different names
223+
std::string controller_name1 = "test_controller1";
224+
std::string controller_name2 = "test_controller2";
225+
ASSERT_NO_THROW(cm_->load_controller(controller_name1, controller_type));
226+
ASSERT_NO_THROW(cm_->load_controller(
227+
controller_name2, test_controller_with_interfaces::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME));
228+
ASSERT_EQ(2u, cm_->get_loaded_controllers().size());
229+
controller_manager::ControllerSpec abstract_test_controller1 = cm_->get_loaded_controllers()[0];
230+
controller_manager::ControllerSpec abstract_test_controller2 = cm_->get_loaded_controllers()[1];
231+
232+
// Configure controllers
233+
ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name1));
234+
ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name2));
235+
236+
ASSERT_EQ(
237+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
238+
abstract_test_controller1.c->get_state().id());
239+
ASSERT_EQ(
240+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
241+
abstract_test_controller2.c->get_state().id());
242+
243+
{ // Test starting the first controller
244+
RCLCPP_INFO(cm_->get_logger(), "Starting controller #1");
245+
std::vector<std::string> start_controllers = {controller_name1};
246+
std::vector<std::string> stop_controllers = {};
247+
auto switch_future = std::async(
248+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
249+
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
250+
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
251+
<< "switch_controller should be blocking until next update cycle";
252+
ControllerManagerRunner cm_runner(this);
253+
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
254+
ASSERT_EQ(
255+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
256+
abstract_test_controller1.c->get_state().id());
257+
ASSERT_EQ(
258+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
259+
abstract_test_controller2.c->get_state().id());
260+
}
261+
262+
{ // Test starting the second controller when the first is running
263+
// Fails as they have the same command interface
264+
RCLCPP_INFO(cm_->get_logger(), "Starting controller #2");
265+
std::vector<std::string> start_controllers = {controller_name2};
266+
std::vector<std::string> stop_controllers = {};
267+
auto switch_future = std::async(
268+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
269+
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
270+
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
271+
<< "switch_controller should be blocking until next update cycle";
272+
ControllerManagerRunner cm_runner(this);
273+
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
274+
ASSERT_EQ(
275+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
276+
abstract_test_controller1.c->get_state().id());
277+
ASSERT_EQ(
278+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
279+
abstract_test_controller2.c->get_state().id());
280+
}
281+
}

‎ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -306,6 +306,59 @@ const auto hardware_resources =
306306
</ros2_control>
307307
)";
308308

309+
const auto hardware_resources_with_exclusive_interface =
310+
R"(
311+
<ros2_control name="TestActuatorHardware1" type="actuator">
312+
<hardware>
313+
<plugin>test_actuator_exclusive_interfaces</plugin>
314+
</hardware>
315+
<joint name="joint1">
316+
<command_interface name="position"/>
317+
<command_interface name="velocity"/>
318+
<command_interface name="effort"/>
319+
<state_interface name="position"/>
320+
<state_interface name="velocity"/>
321+
<state_interface name="effort"/>
322+
</joint>
323+
</ros2_control>
324+
<ros2_control name="TestActuatorHardware2" type="actuator">
325+
<hardware>
326+
<plugin>test_actuator_exclusive_interfaces</plugin>
327+
</hardware>
328+
<joint name="joint2">
329+
<command_interface name="position"/>
330+
<command_interface name="velocity"/>
331+
<command_interface name="effort"/>
332+
<state_interface name="position"/>
333+
<state_interface name="velocity"/>
334+
<state_interface name="effort"/>
335+
</joint>
336+
</ros2_control>
337+
<ros2_control name="TestActuatorHardware3" type="actuator">
338+
<hardware>
339+
<plugin>test_actuator_exclusive_interfaces</plugin>
340+
</hardware>
341+
<joint name="joint3">
342+
<command_interface name="position"/>
343+
<command_interface name="velocity"/>
344+
<command_interface name="effort"/>
345+
<state_interface name="position"/>
346+
<state_interface name="velocity"/>
347+
<state_interface name="effort"/>
348+
</joint>
349+
</ros2_control>
350+
<ros2_control name="TestSensorHardware" type="sensor">
351+
<hardware>
352+
<plugin>test_sensor</plugin>
353+
<param name="example_param_write_for_sec">2</param>
354+
<param name="example_param_read_for_sec">2</param>
355+
</hardware>
356+
<sensor name="sensor1">
357+
<state_interface name="velocity"/>
358+
</sensor>
359+
</ros2_control>
360+
)";
361+
309362
const auto uninitializable_hardware_resources =
310363
R"(
311364
<ros2_control name="TestUninitializableActuatorHardware" type="actuator">

0 commit comments

Comments
 (0)