Skip to content

Commit

Permalink
Fix multiple chainable controller activation bug (#1401)
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor committed Feb 22, 2024
1 parent c4affe4 commit 1cc73c2
Show file tree
Hide file tree
Showing 2 changed files with 207 additions and 3 deletions.
13 changes: 10 additions & 3 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -840,6 +840,12 @@ void ControllerManager::clear_requests()
{
deactivate_request_.clear();
activate_request_.clear();
// Set these interfaces as unavailable when clearing requests to avoid leaving them in available
// state without the controller being in active state
for (const auto & controller_name : to_chained_mode_request_)
{
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
}
to_chained_mode_request_.clear();
from_chained_mode_request_.clear();
activate_command_interface_request_.clear();
Expand Down Expand Up @@ -1429,9 +1435,6 @@ void ControllerManager::switch_chained_mode(
auto controller = found_it->c;
if (!is_controller_active(*controller))
{
// if it is a chainable controller, make the reference interfaces available on preactivation
// (This is needed when you activate a couple of chainable controller altogether)
resource_manager_->make_controller_reference_interfaces_available(controller_name);
if (!controller->set_chained_mode(to_chained_mode))
{
RCLCPP_ERROR(
Expand Down Expand Up @@ -2368,6 +2371,10 @@ controller_interface::return_type ControllerManager::check_following_controllers
if (found_it == to_chained_mode_request_.end())
{
to_chained_mode_request_.push_back(following_ctrl_it->info.name);
// if it is a chainable controller, make the reference interfaces available on preactivation
// (This is needed when you activate a couple of chainable controller altogether)
resource_manager_->make_controller_reference_interfaces_available(
following_ctrl_it->info.name);
RCLCPP_DEBUG(
get_logger(), "Adding controller '%s' in 'to chained mode' request.",
following_ctrl_it->info.name.c_str());
Expand Down
197 changes: 197 additions & 0 deletions controller_manager/test/test_controller_manager_srvs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1729,3 +1729,200 @@ TEST_F(TestControllerManagerSrvs, list_hardware_interfaces_srv)
}
}
}

TEST_F(TestControllerManagerSrvs, activate_chained_controllers_one_by_one)
{
/// The simulated controller chaining is:
/// test_controller_name -> chain_ctrl_2 -> chain_ctrl_1
///
/// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported
/// from the controller B (or) the controller B is utilizing the expected interfaces exported from
/// the controller A

// create server client and request
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
srv_executor.add_node(srv_node);
rclcpp::Client<ListControllers>::SharedPtr client =
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
auto request = std::make_shared<ListControllers::Request>();

// create set of chained controllers
static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1";
static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2";
auto test_chained_controller_1 = std::make_shared<TestChainableController>();
controller_interface::InterfaceConfiguration chained_cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
controller_interface::InterfaceConfiguration chained_state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg);
test_chained_controller_1->set_state_interface_configuration(chained_state_cfg);
test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"});

auto test_chained_controller_2 = std::make_shared<TestChainableController>();
chained_cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}};
test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg);
test_chained_controller_2->set_state_interface_configuration(chained_state_cfg);
test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"});

// create non-chained controller
auto test_controller = std::make_shared<TestController>();
controller_interface::InterfaceConfiguration cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position",
std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/velocity", "joint2/velocity"}};
controller_interface::InterfaceConfiguration state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_controller->set_command_interface_configuration(cmd_cfg);
test_controller->set_state_interface_configuration(state_cfg);
// add controllers
cm_->add_controller(
test_chained_controller_1, TEST_CHAINED_CONTROLLER_1,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
test_chained_controller_2, TEST_CHAINED_CONTROLLER_2,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
// get controller list before configure
auto result = call_service_and_wait(*client, request, srv_executor);
// check chainable controller
ASSERT_EQ(3u, result->controller.size());
ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_1);
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2);
// check test controller
ASSERT_EQ(result->controller[1].name, "test_controller_name");

// configure controllers
for (const auto & controller :
{TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME,
TEST_CHAINED_CONTROLLER_2})
{
cm_->configure_controller(controller);
}

// get controller list after configure
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(3u, result->controller.size());

// reordered controllers
ASSERT_EQ(result->controller[0].name, "test_controller_name");
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_2);
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1);

// activate controllers one by one
auto res1 = cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
ASSERT_EQ(res1, controller_interface::return_type::OK);
auto res2 = cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_2}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
ASSERT_EQ(res2, controller_interface::return_type::OK);
auto res3 = cm_->switch_controller(
{test_controller::TEST_CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
ASSERT_EQ(res3, controller_interface::return_type::OK);

RCLCPP_ERROR(srv_node->get_logger(), "Check successful!");
}

TEST_F(TestControllerManagerSrvs, activate_chained_controllers_all_at_once)
{
/// The simulated controller chaining is:
/// test_controller_name -> chain_ctrl_2 -> chain_ctrl_1
///
/// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported
/// from the controller B (or) the controller B is utilizing the expected interfaces exported from
/// the controller A

// create server client and request
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
srv_executor.add_node(srv_node);
rclcpp::Client<ListControllers>::SharedPtr client =
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
auto request = std::make_shared<ListControllers::Request>();

// create set of chained controllers
static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1";
static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2";
auto test_chained_controller_1 = std::make_shared<TestChainableController>();
controller_interface::InterfaceConfiguration chained_cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
controller_interface::InterfaceConfiguration chained_state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg);
test_chained_controller_1->set_state_interface_configuration(chained_state_cfg);
test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"});

auto test_chained_controller_2 = std::make_shared<TestChainableController>();
chained_cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}};
test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg);
test_chained_controller_2->set_state_interface_configuration(chained_state_cfg);
test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"});

// create non-chained controller
auto test_controller = std::make_shared<TestController>();
controller_interface::InterfaceConfiguration cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position",
std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/velocity", "joint2/velocity"}};
controller_interface::InterfaceConfiguration state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_controller->set_command_interface_configuration(cmd_cfg);
test_controller->set_state_interface_configuration(state_cfg);
// add controllers
cm_->add_controller(
test_chained_controller_1, TEST_CHAINED_CONTROLLER_1,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
test_chained_controller_2, TEST_CHAINED_CONTROLLER_2,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
// get controller list before configure
auto result = call_service_and_wait(*client, request, srv_executor);
// check chainable controller
ASSERT_EQ(3u, result->controller.size());
ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_1);
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2);
// check test controller
ASSERT_EQ(result->controller[1].name, "test_controller_name");

// configure controllers
for (const auto & controller :
{TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME,
TEST_CHAINED_CONTROLLER_2})
{
cm_->configure_controller(controller);
}

// get controller list after configure
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(3u, result->controller.size());

// reordered controllers
ASSERT_EQ(result->controller[0].name, "test_controller_name");
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_2);
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1);

// activate controllers all at once
auto res = cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME},
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));
ASSERT_EQ(res, controller_interface::return_type::OK);

RCLCPP_ERROR(srv_node->get_logger(), "Check successful!");
}

0 comments on commit 1cc73c2

Please sign in to comment.