Skip to content

Commit 1cc73c2

Browse files
saikishorsaikishor
authored
Fix multiple chainable controller activation bug (ros-controls#1401)
1 parent c4affe4 commit 1cc73c2

File tree

2 files changed

+207
-3
lines changed

2 files changed

+207
-3
lines changed

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 10 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -840,6 +840,12 @@ void ControllerManager::clear_requests()
840840
{
841841
deactivate_request_.clear();
842842
activate_request_.clear();
843+
// Set these interfaces as unavailable when clearing requests to avoid leaving them in available
844+
// state without the controller being in active state
845+
for (const auto & controller_name : to_chained_mode_request_)
846+
{
847+
resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
848+
}
843849
to_chained_mode_request_.clear();
844850
from_chained_mode_request_.clear();
845851
activate_command_interface_request_.clear();
@@ -1429,9 +1435,6 @@ void ControllerManager::switch_chained_mode(
14291435
auto controller = found_it->c;
14301436
if (!is_controller_active(*controller))
14311437
{
1432-
// if it is a chainable controller, make the reference interfaces available on preactivation
1433-
// (This is needed when you activate a couple of chainable controller altogether)
1434-
resource_manager_->make_controller_reference_interfaces_available(controller_name);
14351438
if (!controller->set_chained_mode(to_chained_mode))
14361439
{
14371440
RCLCPP_ERROR(
@@ -2368,6 +2371,10 @@ controller_interface::return_type ControllerManager::check_following_controllers
23682371
if (found_it == to_chained_mode_request_.end())
23692372
{
23702373
to_chained_mode_request_.push_back(following_ctrl_it->info.name);
2374+
// if it is a chainable controller, make the reference interfaces available on preactivation
2375+
// (This is needed when you activate a couple of chainable controller altogether)
2376+
resource_manager_->make_controller_reference_interfaces_available(
2377+
following_ctrl_it->info.name);
23712378
RCLCPP_DEBUG(
23722379
get_logger(), "Adding controller '%s' in 'to chained mode' request.",
23732380
following_ctrl_it->info.name.c_str());

‎controller_manager/test/test_controller_manager_srvs.cpp‎

Lines changed: 197 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1729,3 +1729,200 @@ TEST_F(TestControllerManagerSrvs, list_hardware_interfaces_srv)
17291729
}
17301730
}
17311731
}
1732+
1733+
TEST_F(TestControllerManagerSrvs, activate_chained_controllers_one_by_one)
1734+
{
1735+
/// The simulated controller chaining is:
1736+
/// test_controller_name -> chain_ctrl_2 -> chain_ctrl_1
1737+
///
1738+
/// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported
1739+
/// from the controller B (or) the controller B is utilizing the expected interfaces exported from
1740+
/// the controller A
1741+
1742+
// create server client and request
1743+
rclcpp::executors::SingleThreadedExecutor srv_executor;
1744+
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
1745+
srv_executor.add_node(srv_node);
1746+
rclcpp::Client<ListControllers>::SharedPtr client =
1747+
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
1748+
auto request = std::make_shared<ListControllers::Request>();
1749+
1750+
// create set of chained controllers
1751+
static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1";
1752+
static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2";
1753+
auto test_chained_controller_1 = std::make_shared<TestChainableController>();
1754+
controller_interface::InterfaceConfiguration chained_cmd_cfg = {
1755+
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
1756+
controller_interface::InterfaceConfiguration chained_state_cfg = {
1757+
controller_interface::interface_configuration_type::INDIVIDUAL,
1758+
{"joint1/position", "joint1/velocity"}};
1759+
test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg);
1760+
test_chained_controller_1->set_state_interface_configuration(chained_state_cfg);
1761+
test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"});
1762+
1763+
auto test_chained_controller_2 = std::make_shared<TestChainableController>();
1764+
chained_cmd_cfg = {
1765+
controller_interface::interface_configuration_type::INDIVIDUAL,
1766+
{std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}};
1767+
test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg);
1768+
test_chained_controller_2->set_state_interface_configuration(chained_state_cfg);
1769+
test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"});
1770+
1771+
// create non-chained controller
1772+
auto test_controller = std::make_shared<TestController>();
1773+
controller_interface::InterfaceConfiguration cmd_cfg = {
1774+
controller_interface::interface_configuration_type::INDIVIDUAL,
1775+
{std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position",
1776+
std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/velocity", "joint2/velocity"}};
1777+
controller_interface::InterfaceConfiguration state_cfg = {
1778+
controller_interface::interface_configuration_type::INDIVIDUAL,
1779+
{"joint1/position", "joint1/velocity"}};
1780+
test_controller->set_command_interface_configuration(cmd_cfg);
1781+
test_controller->set_state_interface_configuration(state_cfg);
1782+
// add controllers
1783+
cm_->add_controller(
1784+
test_chained_controller_1, TEST_CHAINED_CONTROLLER_1,
1785+
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
1786+
cm_->add_controller(
1787+
test_controller, test_controller::TEST_CONTROLLER_NAME,
1788+
test_controller::TEST_CONTROLLER_CLASS_NAME);
1789+
cm_->add_controller(
1790+
test_chained_controller_2, TEST_CHAINED_CONTROLLER_2,
1791+
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
1792+
// get controller list before configure
1793+
auto result = call_service_and_wait(*client, request, srv_executor);
1794+
// check chainable controller
1795+
ASSERT_EQ(3u, result->controller.size());
1796+
ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_1);
1797+
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2);
1798+
// check test controller
1799+
ASSERT_EQ(result->controller[1].name, "test_controller_name");
1800+
1801+
// configure controllers
1802+
for (const auto & controller :
1803+
{TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME,
1804+
TEST_CHAINED_CONTROLLER_2})
1805+
{
1806+
cm_->configure_controller(controller);
1807+
}
1808+
1809+
// get controller list after configure
1810+
result = call_service_and_wait(*client, request, srv_executor);
1811+
ASSERT_EQ(3u, result->controller.size());
1812+
1813+
// reordered controllers
1814+
ASSERT_EQ(result->controller[0].name, "test_controller_name");
1815+
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_2);
1816+
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1);
1817+
1818+
// activate controllers one by one
1819+
auto res1 = cm_->switch_controller(
1820+
{TEST_CHAINED_CONTROLLER_1}, {},
1821+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
1822+
ASSERT_EQ(res1, controller_interface::return_type::OK);
1823+
auto res2 = cm_->switch_controller(
1824+
{TEST_CHAINED_CONTROLLER_2}, {},
1825+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
1826+
ASSERT_EQ(res2, controller_interface::return_type::OK);
1827+
auto res3 = cm_->switch_controller(
1828+
{test_controller::TEST_CONTROLLER_NAME}, {},
1829+
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
1830+
ASSERT_EQ(res3, controller_interface::return_type::OK);
1831+
1832+
RCLCPP_ERROR(srv_node->get_logger(), "Check successful!");
1833+
}
1834+
1835+
TEST_F(TestControllerManagerSrvs, activate_chained_controllers_all_at_once)
1836+
{
1837+
/// The simulated controller chaining is:
1838+
/// test_controller_name -> chain_ctrl_2 -> chain_ctrl_1
1839+
///
1840+
/// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported
1841+
/// from the controller B (or) the controller B is utilizing the expected interfaces exported from
1842+
/// the controller A
1843+
1844+
// create server client and request
1845+
rclcpp::executors::SingleThreadedExecutor srv_executor;
1846+
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
1847+
srv_executor.add_node(srv_node);
1848+
rclcpp::Client<ListControllers>::SharedPtr client =
1849+
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
1850+
auto request = std::make_shared<ListControllers::Request>();
1851+
1852+
// create set of chained controllers
1853+
static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1";
1854+
static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2";
1855+
auto test_chained_controller_1 = std::make_shared<TestChainableController>();
1856+
controller_interface::InterfaceConfiguration chained_cmd_cfg = {
1857+
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
1858+
controller_interface::InterfaceConfiguration chained_state_cfg = {
1859+
controller_interface::interface_configuration_type::INDIVIDUAL,
1860+
{"joint1/position", "joint1/velocity"}};
1861+
test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg);
1862+
test_chained_controller_1->set_state_interface_configuration(chained_state_cfg);
1863+
test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"});
1864+
1865+
auto test_chained_controller_2 = std::make_shared<TestChainableController>();
1866+
chained_cmd_cfg = {
1867+
controller_interface::interface_configuration_type::INDIVIDUAL,
1868+
{std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}};
1869+
test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg);
1870+
test_chained_controller_2->set_state_interface_configuration(chained_state_cfg);
1871+
test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"});
1872+
1873+
// create non-chained controller
1874+
auto test_controller = std::make_shared<TestController>();
1875+
controller_interface::InterfaceConfiguration cmd_cfg = {
1876+
controller_interface::interface_configuration_type::INDIVIDUAL,
1877+
{std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position",
1878+
std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/velocity", "joint2/velocity"}};
1879+
controller_interface::InterfaceConfiguration state_cfg = {
1880+
controller_interface::interface_configuration_type::INDIVIDUAL,
1881+
{"joint1/position", "joint1/velocity"}};
1882+
test_controller->set_command_interface_configuration(cmd_cfg);
1883+
test_controller->set_state_interface_configuration(state_cfg);
1884+
// add controllers
1885+
cm_->add_controller(
1886+
test_chained_controller_1, TEST_CHAINED_CONTROLLER_1,
1887+
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
1888+
cm_->add_controller(
1889+
test_controller, test_controller::TEST_CONTROLLER_NAME,
1890+
test_controller::TEST_CONTROLLER_CLASS_NAME);
1891+
cm_->add_controller(
1892+
test_chained_controller_2, TEST_CHAINED_CONTROLLER_2,
1893+
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
1894+
// get controller list before configure
1895+
auto result = call_service_and_wait(*client, request, srv_executor);
1896+
// check chainable controller
1897+
ASSERT_EQ(3u, result->controller.size());
1898+
ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_1);
1899+
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_2);
1900+
// check test controller
1901+
ASSERT_EQ(result->controller[1].name, "test_controller_name");
1902+
1903+
// configure controllers
1904+
for (const auto & controller :
1905+
{TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME,
1906+
TEST_CHAINED_CONTROLLER_2})
1907+
{
1908+
cm_->configure_controller(controller);
1909+
}
1910+
1911+
// get controller list after configure
1912+
result = call_service_and_wait(*client, request, srv_executor);
1913+
ASSERT_EQ(3u, result->controller.size());
1914+
1915+
// reordered controllers
1916+
ASSERT_EQ(result->controller[0].name, "test_controller_name");
1917+
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_2);
1918+
ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1);
1919+
1920+
// activate controllers all at once
1921+
auto res = cm_->switch_controller(
1922+
{TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_2, test_controller::TEST_CONTROLLER_NAME},
1923+
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
1924+
rclcpp::Duration(0, 0));
1925+
ASSERT_EQ(res, controller_interface::return_type::OK);
1926+
1927+
RCLCPP_ERROR(srv_node->get_logger(), "Check successful!");
1928+
}

0 commit comments

Comments
 (0)