diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
index 72b63962da1..689e7c72448 100644
--- a/controller_manager/CMakeLists.txt
+++ b/controller_manager/CMakeLists.txt
@@ -183,17 +183,13 @@ if(BUILD_TESTING)
target_link_libraries(test_hardware_spawner ${PROJECT_NAME})
ament_target_dependencies(test_hardware_spawner ros2_control_test_assets)
- install(FILES test/test_controller_spawner_with_type.yaml
- DESTINATION test)
-
- install(FILES test/test_controller_spawner_with_basic_controllers.yaml
- DESTINATION test)
-
- install(FILES test/test_controller_overriding_parameters.yaml
- DESTINATION test)
-
- install(FILES test/test_controller_spawner_wildcard_entries.yaml
- DESTINATION test)
+ install(FILES
+ test/test_controller_spawner_with_type.yaml
+ test/test_controller_spawner_with_basic_controllers.yaml
+ test/test_controller_overriding_parameters.yaml
+ test/test_controller_spawner_wildcard_entries.yaml
+ test/test_controller_spawner_with_interfaces.yaml
+ DESTINATION test)
ament_add_gmock(
test_hardware_management_srvs
diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
index c7fab714037..9f52fae2d35 100644
--- a/controller_manager/controller_manager/spawner.py
+++ b/controller_manager/controller_manager/spawner.py
@@ -265,7 +265,7 @@ def main(args=None):
)
if not ret.ok:
node.get_logger().error(
- bcolors.FAIL + "Failed to activate controller" + bcolors.ENDC
+ f"{bcolors.FAIL}Failed to activate controller : {controller_name}{bcolors.ENDC}"
)
return 1
@@ -290,7 +290,7 @@ def main(args=None):
)
if not ret.ok:
node.get_logger().error(
- bcolors.FAIL + "Failed to activate the parsed controllers list" + bcolors.ENDC
+ f"{bcolors.FAIL}Failed to activate the parsed controllers list : {controller_names}{bcolors.ENDC}"
)
return 1
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index 1f387b89ee7..29b8ac2e54f 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -1264,6 +1264,7 @@ controller_interface::return_type ControllerManager::switch_controller(
to = controllers;
// update the claimed interface controller info
+ auto switch_result = controller_interface::return_type::OK;
for (auto & controller : to)
{
if (is_controller_active(controller.c))
@@ -1284,6 +1285,32 @@ controller_interface::return_type ControllerManager::switch_controller(
{
controller.info.claimed_interfaces.clear();
}
+ if (
+ std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) !=
+ activate_request_.end())
+ {
+ if (!is_controller_active(controller.c))
+ {
+ RCLCPP_ERROR(
+ get_logger(), "Could not activate controller : '%s'", controller.info.name.c_str());
+ switch_result = controller_interface::return_type::ERROR;
+ }
+ }
+ /// @note The following is the case of the real controllers that are deactivated and doesn't
+ /// include the chained controllers that are deactivated and activated
+ if (
+ std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name) !=
+ deactivate_request_.end() &&
+ std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) ==
+ activate_request_.end())
+ {
+ if (is_controller_active(controller.c))
+ {
+ RCLCPP_ERROR(
+ get_logger(), "Could not deactivate controller : '%s'", controller.info.name.c_str());
+ switch_result = controller_interface::return_type::ERROR;
+ }
+ }
}
// switch lists
@@ -1293,8 +1320,10 @@ controller_interface::return_type ControllerManager::switch_controller(
clear_requests();
- RCLCPP_DEBUG(get_logger(), "Successfully switched controllers");
- return controller_interface::return_type::OK;
+ RCLCPP_DEBUG_EXPRESSION(
+ get_logger(), switch_result == controller_interface::return_type::OK,
+ "Successfully switched controllers");
+ return switch_result;
}
controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_controller_impl(
@@ -1518,6 +1547,7 @@ void ControllerManager::activate_controllers()
get_logger(),
"Resource conflict for controller '%s'. Command interface '%s' is already claimed.",
controller_name.c_str(), command_interface.c_str());
+ command_loans.clear();
assignment_successful = false;
break;
}
@@ -1529,6 +1559,7 @@ void ControllerManager::activate_controllers()
{
RCLCPP_ERROR(
get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what());
+ command_loans.clear();
assignment_successful = false;
break;
}
diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
index b6eac9b689b..e7eae228378 100644
--- a/controller_manager/test/test_controller/test_controller.cpp
+++ b/controller_manager/test/test_controller/test_controller.cpp
@@ -64,7 +64,7 @@ controller_interface::return_type TestController::update(
for (size_t i = 0; i < command_interfaces_.size(); ++i)
{
- RCLCPP_INFO(
+ RCLCPP_DEBUG(
get_node()->get_logger(), "Setting value of command interface '%s' to %f",
command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);
command_interfaces_[i].set_value(external_commands_for_testing_[i]);
@@ -77,6 +77,48 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; }
CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
{
+ auto ctrl_node = get_node();
+ if (!ctrl_node->has_parameter("command_interfaces"))
+ {
+ ctrl_node->declare_parameter("command_interfaces", std::vector<std::string>({}));
+ }
+ if (!ctrl_node->has_parameter("state_interfaces"))
+ {
+ ctrl_node->declare_parameter("state_interfaces", std::vector<std::string>({}));
+ }
+ const std::vector<std::string> command_interfaces =
+ ctrl_node->get_parameter("command_interfaces").as_string_array();
+ const std::vector<std::string> state_interfaces =
+ ctrl_node->get_parameter("state_interfaces").as_string_array();
+ if (!command_interfaces.empty() || !state_interfaces.empty())
+ {
+ cmd_iface_cfg_.names.clear();
+ state_iface_cfg_.names.clear();
+ for (const auto & cmd_itf : command_interfaces)
+ {
+ cmd_iface_cfg_.names.push_back(cmd_itf);
+ }
+ cmd_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+ external_commands_for_testing_.resize(command_interfaces.size(), 0.0);
+ for (const auto & state_itf : state_interfaces)
+ {
+ state_iface_cfg_.names.push_back(state_itf);
+ }
+ state_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+ }
+
+ const std::string service_name = get_node()->get_name() + std::string("/set_bool");
+ service_ = get_node()->create_service<example_interfaces::srv::SetBool>(
+ service_name,
+ [this](
+ const std::shared_ptr<example_interfaces::srv::SetBool::Request> request,
+ std::shared_ptr<example_interfaces::srv::SetBool::Response> response)
+ {
+ RCLCPP_INFO_STREAM(
+ get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data);
+ response->success = request->data;
+ });
+
return CallbackReturn::SUCCESS;
}
diff --git a/controller_manager/test/test_controller_spawner_with_interfaces.yaml b/controller_manager/test/test_controller_spawner_with_interfaces.yaml
new file mode 100644
index 00000000000..05b650c2322
--- /dev/null
+++ b/controller_manager/test/test_controller_spawner_with_interfaces.yaml
@@ -0,0 +1,25 @@
+ctrl_with_joint2_command_interface:
+ ros__parameters:
+ type: "controller_manager/test_controller"
+ command_interfaces:
+ - "joint2/velocity"
+
+ctrl_with_joint1_command_interface:
+ ros__parameters:
+ type: "controller_manager/test_controller"
+ command_interfaces:
+ - "joint1/position"
+
+ctrl_with_joint1_and_joint2_command_interfaces:
+ ros__parameters:
+ type: "controller_manager/test_controller"
+ command_interfaces:
+ - "joint1/position"
+ - "joint2/velocity"
+
+ctrl_with_state_interfaces:
+ ros__parameters:
+ type: "controller_manager/test_controller"
+ state_interfaces:
+ - "joint1/position"
+ - "joint2/position"
diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp
index 3ee141edaad..b72384b7fd5 100644
--- a/controller_manager/test/test_release_interfaces.cpp
+++ b/controller_manager/test/test_release_interfaces.cpp
@@ -86,7 +86,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner(this);
- EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+ EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_state().id());
@@ -190,7 +190,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner(this);
- EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+ EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_state().id());
diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp
index 8a0c35a6a30..6a91531357c 100644
--- a/controller_manager/test/test_spawner_unspawner.cpp
+++ b/controller_manager/test/test_spawner_unspawner.cpp
@@ -411,6 +411,106 @@ TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name)
verify_ctrl_parameter(wildcard_ctrl_3.c->get_node(), true);
}
+TEST_F(TestLoadController, spawner_test_failed_activation_of_controllers)
+{
+ const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+ "/test/test_controller_spawner_with_interfaces.yaml";
+
+ ControllerManagerRunner cm_runner(this);
+ // Provide controller type via the parsed file
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_joint1_command_interface ctrl_with_joint2_command_interface -c "
+ "test_controller_manager "
+ "--controller-manager-timeout 1.0 "
+ "-p " +
+ test_file_path),
+ 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+
+ auto ctrl_with_joint2_command_interface = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(ctrl_with_joint2_command_interface.info.name, "ctrl_with_joint2_command_interface");
+ ASSERT_EQ(
+ ctrl_with_joint2_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ EXPECT_EQ(
+ ctrl_with_joint2_command_interface.c->get_lifecycle_state().id(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+ ASSERT_EQ(
+ ctrl_with_joint2_command_interface.c->command_interface_configuration().names.size(), 1ul);
+ ASSERT_THAT(
+ ctrl_with_joint2_command_interface.c->command_interface_configuration().names,
+ std::vector<std::string>({"joint2/velocity"}));
+
+ auto ctrl_with_joint1_command_interface = cm_->get_loaded_controllers()[1];
+ ASSERT_EQ(ctrl_with_joint1_command_interface.info.name, "ctrl_with_joint1_command_interface");
+ ASSERT_EQ(
+ ctrl_with_joint1_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ EXPECT_EQ(
+ ctrl_with_joint1_command_interface.c->get_lifecycle_state().id(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+ ASSERT_EQ(
+ ctrl_with_joint1_command_interface.c->command_interface_configuration().names.size(), 1ul);
+ ASSERT_THAT(
+ ctrl_with_joint1_command_interface.c->command_interface_configuration().names,
+ std::vector<std::string>({"joint1/position"}));
+
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager "
+ "--controller-manager-timeout 1.0 "
+ "-p " +
+ test_file_path),
+ 256)
+ << "Should fail as the ctrl_with_joint1_command_interface and "
+ "ctrl_with_joint2_command_interface are active";
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+
+ auto ctrl_with_joint1_and_joint2_command_interfaces = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(
+ ctrl_with_joint1_and_joint2_command_interfaces.info.name,
+ "ctrl_with_joint1_and_joint2_command_interfaces");
+ ASSERT_EQ(
+ ctrl_with_joint1_and_joint2_command_interfaces.info.type,
+ test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(
+ ctrl_with_joint1_and_joint2_command_interfaces.c->get_lifecycle_state().id(),
+ lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+ ASSERT_EQ(
+ ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration()
+ .names.size(),
+ 2ul);
+ ASSERT_THAT(
+ ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration().names,
+ std::vector<std::string>({"joint1/position", "joint2/velocity"}));
+
+ EXPECT_EQ(call_unspawner("ctrl_with_joint1_command_interface -c test_controller_manager"), 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager "
+ "--controller-manager-timeout 1.0 "
+ "-p " +
+ test_file_path),
+ 256)
+ << "Should fail as the ctrl_with_joint2_command_interface is still active";
+
+ EXPECT_EQ(call_unspawner("ctrl_with_joint2_command_interface -c test_controller_manager"), 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager "
+ "--controller-manager-timeout 1.0 "
+ "-p " +
+ test_file_path),
+ 0)
+ << "Should pass as the ctrl_with_joint1_command_interface and "
+ "ctrl_with_joint2_command_interface are inactive";
+}
+
TEST_F(TestLoadController, unload_on_kill)
{
// Launch spawner with unload on kill