Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix the controller deactivation on the control cycles returning ERROR #1756

Open
wants to merge 12 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions controller_manager/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -192,6 +192,7 @@ if(BUILD_TESTING)
)
target_link_libraries(test_release_interfaces
controller_manager
test_controller
test_controller_with_interfaces
ros2_control_test_assets::ros2_control_test_assets
)
Expand Down
84 changes: 62 additions & 22 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -207,6 +207,44 @@ void get_active_controllers_using_command_interfaces_of_controller(
}
}

void extract_command_interfaces_for_controller(
const controller_manager::ControllerSpec & ctrl,
const hardware_interface::ResourceManager & resource_manager,
std::vector<std::string> & request_interface_list)
{
auto command_interface_config = ctrl.c->command_interface_configuration();
std::vector<std::string> command_interface_names = {};
if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
{
command_interface_names = resource_manager.available_command_interfaces();
}
if (
command_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL)
{
command_interface_names = command_interface_config.names;
}
request_interface_list.insert(
request_interface_list.end(), command_interface_names.begin(), command_interface_names.end());
}

void get_controller_list_command_interfaces(
const std::vector<std::string> & controllers_list,
const std::vector<controller_manager::ControllerSpec> & controllers_spec,
const hardware_interface::ResourceManager & resource_manager,
std::vector<std::string> & request_interface_list)
{
for (const auto & controller_name : controllers_list)
{
auto found_it = std::find_if(
controllers_spec.begin(), controllers_spec.end(),
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
if (found_it != controllers_spec.end())
{
extract_command_interfaces_for_controller(
*found_it, resource_manager, request_interface_list);
}
}
}
} // namespace

namespace controller_manager
Expand Down Expand Up @@ -1335,33 +1373,15 @@ controller_interface::return_type ControllerManager::switch_controller(
activate_request_.erase(activate_list_it);
}

const auto extract_interfaces_for_controller =
[this](const ControllerSpec ctrl, std::vector<std::string> & request_interface_list)
{
auto command_interface_config = ctrl.c->command_interface_configuration();
std::vector<std::string> command_interface_names = {};
if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
{
command_interface_names = resource_manager_->available_command_interfaces();
}
if (
command_interface_config.type ==
controller_interface::interface_configuration_type::INDIVIDUAL)
{
command_interface_names = command_interface_config.names;
}
request_interface_list.insert(
request_interface_list.end(), command_interface_names.begin(),
command_interface_names.end());
};

if (in_activate_list)
{
extract_interfaces_for_controller(controller, activate_command_interface_request_);
extract_command_interfaces_for_controller(
controller, *resource_manager_, activate_command_interface_request_);
}
if (in_deactivate_list)
{
extract_interfaces_for_controller(controller, deactivate_command_interface_request_);
extract_command_interfaces_for_controller(
controller, *resource_manager_, deactivate_command_interface_request_);
}

// cache mapping between hardware and controllers for stopping when read/write error happens
Expand Down Expand Up @@ -2592,6 +2612,26 @@ controller_interface::return_type ControllerManager::update(
RCLCPP_ERROR(
get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str());
}
std::vector<std::string> failed_controller_interfaces, fallback_controller_interfaces;
failed_controller_interfaces.reserve(500);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Isn't this in the update method? Can we do this ahead of time?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@destogl I was handling it in a different PR: #1801

get_controller_list_command_interfaces(
failed_controllers_list, rt_controller_list, *resource_manager_,
failed_controller_interfaces);
get_controller_list_command_interfaces(
cumulative_fallback_controllers, rt_controller_list, *resource_manager_,
fallback_controller_interfaces);
if (!failed_controller_interfaces.empty())
{
if (!(resource_manager_->prepare_command_mode_switch(
fallback_controller_interfaces, failed_controller_interfaces) &&
resource_manager_->perform_command_mode_switch(
fallback_controller_interfaces, failed_controller_interfaces)))
{
RCLCPP_ERROR(
get_logger(),
"Error while attempting mode switch when deactivating controllers in update cycle!");
}
}
deactivate_controllers(rt_controller_list, active_controllers_using_interfaces);
if (!cumulative_fallback_controllers.empty())
{
Expand Down
135 changes: 135 additions & 0 deletions controller_manager/test/test_release_interfaces.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_test_common.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "test_controller/test_controller.hpp"
#include "test_controller_with_interfaces/test_controller_with_interfaces.hpp"

using ::testing::_;
Expand Down Expand Up @@ -197,3 +198,137 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
abstract_test_controller2.c->get_lifecycle_state().id());
}
}

class TestReleaseExclusiveInterfaces
: public ControllerManagerFixture<controller_manager::ControllerManager>
{
public:
TestReleaseExclusiveInterfaces()
: ControllerManagerFixture<controller_manager::ControllerManager>(
std::string(ros2_control_test_assets::urdf_head) +
std::string(ros2_control_test_assets::hardware_resources_with_exclusive_interface) +
std::string(ros2_control_test_assets::urdf_tail))
{
}
};

TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_deactivation_on_update_error)
{
const std::string controller_type = test_controller::TEST_CONTROLLER_CLASS_NAME;

// Load two controllers of different names
const std::string controller_name1 = "test_controller1";
const std::string controller_name2 = "test_controller2";

auto test_controller1 = std::make_shared<test_controller::TestController>();
controller_interface::InterfaceConfiguration cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
controller_interface::InterfaceConfiguration state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_controller1->set_command_interface_configuration(cmd_cfg);
test_controller1->set_state_interface_configuration(state_cfg);
cm_->add_controller(test_controller1, controller_name1, controller_type);

auto test_controller2 = std::make_shared<test_controller::TestController>();
cmd_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}};
state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint2/position", "joint2/velocity"}};
test_controller2->set_command_interface_configuration(cmd_cfg);
test_controller2->set_state_interface_configuration(state_cfg);
cm_->add_controller(test_controller2, controller_name2, controller_type);
ASSERT_EQ(2u, cm_->get_loaded_controllers().size());
controller_manager::ControllerSpec abstract_test_controller1 = cm_->get_loaded_controllers()[0];
controller_manager::ControllerSpec abstract_test_controller2 = cm_->get_loaded_controllers()[1];

// Configure controllers
ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name1));
ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name2));

ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());

{ // Test starting the first and second controller
RCLCPP_INFO(cm_->get_logger(), "Starting controller #1 and #2");
std::vector<std::string> start_controllers = {controller_name1, controller_name2};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
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());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());
}

// As the external command is Nan, the controller update cycle should return an error and
// deactivate the controllers
{
test_controller1->set_external_commands_for_testing({std::numeric_limits<double>::quiet_NaN()});
test_controller2->set_external_commands_for_testing({std::numeric_limits<double>::quiet_NaN()});
ControllerManagerRunner cm_runner(this);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());
}

{ // Test starting both controllers but only the second one will pass as the first one has the
// same external command
test_controller2->set_external_commands_for_testing({0.0});
RCLCPP_INFO(cm_->get_logger(), "Starting controller #1 and #2 again");
std::vector<std::string> start_controllers = {controller_name1, controller_name2};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
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());
std::this_thread::sleep_for(std::chrono::milliseconds(50));
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id())
<< "Controller 1 current state is "
<< abstract_test_controller1.c->get_lifecycle_state().label();
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());
}

{ // Test starting controller 1 and it should work as external command is valid now
test_controller1->set_external_commands_for_testing({0.0});
RCLCPP_INFO(cm_->get_logger(), "Starting controller #1");
std::vector<std::string> start_controllers = {controller_name1};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
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());
std::this_thread::sleep_for(std::chrono::milliseconds(50));
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());
}
}
3 changes: 2 additions & 1 deletion hardware_interface_testing/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,8 @@ endforeach()
add_library(test_components SHARED
test/test_components/test_actuator.cpp
test/test_components/test_sensor.cpp
test/test_components/test_system.cpp)
test/test_components/test_system.cpp
test/test_components/test_actuator_exclusive_interfaces.cpp)
ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets)
install(TARGETS test_components
DESTINATION lib
Expand Down
Loading
Loading