Skip to content

Commit 03372e0

Browse files
saikishorsaikishor
authored
Fix failing controller switch when using ::ALL command interface configuration (#2856)
1 parent fd12e90 commit 03372e0

File tree

3 files changed

+177
-1
lines changed

3 files changed

+177
-1
lines changed

‎controller_manager/src/controller_manager.cpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3965,8 +3965,11 @@ controller_interface::return_type ControllerManager::check_for_interfaces_availa
39653965
RCLCPP_ERROR(get_logger(), "%s", message.c_str());
39663966
return controller_interface::return_type::ERROR;
39673967
}
3968+
const auto cmd_itf_cfg = controller_it->c->command_interface_configuration();
39683969
const auto controller_cmd_interfaces =
3969-
controller_it->c->command_interface_configuration().names;
3970+
(cmd_itf_cfg.type == controller_interface::interface_configuration_type::INDIVIDUAL)
3971+
? cmd_itf_cfg.names
3972+
: controller_it->info.claimed_interfaces;
39703973
for (const auto & cmd_itf : controller_cmd_interfaces)
39713974
{
39723975
future_available_cmd_interfaces.push_back(cmd_itf);

‎controller_manager/test/test_controller/test_controller.cpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -163,6 +163,10 @@ CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*pr
163163

164164
CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
165165
{
166+
if (external_commands_for_testing_.empty())
167+
{
168+
external_commands_for_testing_.resize(command_interfaces_.size(), 0.0);
169+
}
166170
if (activation_processing_time > 0.0)
167171
{
168172
RCLCPP_INFO(

‎controller_manager/test/test_controller_manager_srvs.cpp‎

Lines changed: 169 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2680,6 +2680,175 @@ TEST_F(TestControllerManagerSrvs, switch_controller_test_two_controllers_using_s
26802680
test_controller_2->get_lifecycle_state().id());
26812681
}
26822682

2683+
TEST_F(TestControllerManagerSrvs, switch_controller_test_two_controllers_using_all_interface)
2684+
{
2685+
// create server client and request
2686+
rclcpp::executors::SingleThreadedExecutor srv_executor;
2687+
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
2688+
srv_executor.add_node(srv_node);
2689+
rclcpp::Client<ListControllers>::SharedPtr client =
2690+
srv_node->create_client<ListControllers>("test_controller_manager/list_controllers");
2691+
auto request = std::make_shared<ListControllers::Request>();
2692+
2693+
// create set of controllers
2694+
static constexpr char TEST_CONTROLLER_1[] = "test_controller_1";
2695+
static constexpr char TEST_CONTROLLER_2[] = "test_controller_2";
2696+
2697+
// create non-chained controller
2698+
auto test_controller_1 = std::make_shared<TestController>();
2699+
controller_interface::InterfaceConfiguration cmd_cfg = {
2700+
controller_interface::interface_configuration_type::INDIVIDUAL,
2701+
{"joint1/position", "joint1/max_velocity"}};
2702+
controller_interface::InterfaceConfiguration state_cfg = {
2703+
controller_interface::interface_configuration_type::INDIVIDUAL,
2704+
{"joint1/position", "joint1/velocity"}};
2705+
test_controller_1->set_command_interface_configuration(cmd_cfg);
2706+
test_controller_1->set_state_interface_configuration(state_cfg);
2707+
2708+
auto test_controller_2 = std::make_shared<TestController>();
2709+
cmd_cfg = {controller_interface::interface_configuration_type::ALL, {}};
2710+
state_cfg = {
2711+
controller_interface::interface_configuration_type::INDIVIDUAL,
2712+
{"joint1/position", "joint2/velocity"}};
2713+
test_controller_2->set_command_interface_configuration(cmd_cfg);
2714+
test_controller_2->set_state_interface_configuration(state_cfg);
2715+
2716+
// add controllers
2717+
cm_->add_controller(
2718+
test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME);
2719+
cm_->add_controller(
2720+
test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME);
2721+
2722+
// get controller list before configure
2723+
auto result = call_service_and_wait(*client, request, srv_executor);
2724+
// check chainable controller
2725+
ASSERT_EQ(2u, result->controller.size());
2726+
ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_1);
2727+
ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_2);
2728+
2729+
// configure controllers
2730+
for (const auto & controller : {TEST_CONTROLLER_1, TEST_CONTROLLER_2})
2731+
{
2732+
cm_->configure_controller(controller);
2733+
}
2734+
2735+
// get controller list after configure
2736+
result = call_service_and_wait(*client, request, srv_executor);
2737+
ASSERT_EQ(2u, result->controller.size());
2738+
2739+
// reordered controllers
2740+
ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_2);
2741+
ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_1);
2742+
2743+
// Check individual activation and they should work fine
2744+
auto res = cm_->switch_controller(
2745+
{TEST_CONTROLLER_1}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2746+
rclcpp::Duration(0, 0));
2747+
ASSERT_EQ(res, controller_interface::return_type::OK);
2748+
2749+
ASSERT_EQ(
2750+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2751+
test_controller_1->get_lifecycle_state().id());
2752+
ASSERT_EQ(
2753+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2754+
test_controller_2->get_lifecycle_state().id());
2755+
2756+
cm_->update(cm_->now(), rclcpp::Duration::from_seconds(0.01));
2757+
2758+
res = cm_->switch_controller(
2759+
{}, {TEST_CONTROLLER_1}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2760+
rclcpp::Duration(0, 0));
2761+
ASSERT_EQ(res, controller_interface::return_type::OK);
2762+
2763+
ASSERT_EQ(
2764+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2765+
test_controller_1->get_lifecycle_state().id());
2766+
ASSERT_EQ(
2767+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2768+
test_controller_2->get_lifecycle_state().id());
2769+
cm_->update(cm_->now(), rclcpp::Duration::from_seconds(0.01));
2770+
2771+
// Now test activating controller_2
2772+
res = cm_->switch_controller(
2773+
{TEST_CONTROLLER_2}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2774+
rclcpp::Duration(0, 0));
2775+
ASSERT_EQ(res, controller_interface::return_type::OK);
2776+
2777+
ASSERT_EQ(
2778+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2779+
test_controller_1->get_lifecycle_state().id());
2780+
ASSERT_EQ(
2781+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2782+
test_controller_2->get_lifecycle_state().id());
2783+
cm_->update(cm_->now(), rclcpp::Duration::from_seconds(0.01));
2784+
2785+
res = cm_->switch_controller(
2786+
{}, {TEST_CONTROLLER_2}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2787+
rclcpp::Duration(0, 0));
2788+
ASSERT_EQ(res, controller_interface::return_type::OK);
2789+
2790+
ASSERT_EQ(
2791+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2792+
test_controller_1->get_lifecycle_state().id());
2793+
ASSERT_EQ(
2794+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2795+
test_controller_2->get_lifecycle_state().id());
2796+
cm_->update(cm_->now(), rclcpp::Duration::from_seconds(0.01));
2797+
2798+
// Now try activating both the controllers at once, it should fail as they are using same
2799+
// interface
2800+
res = cm_->switch_controller(
2801+
{TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {},
2802+
controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0));
2803+
ASSERT_EQ(res, controller_interface::return_type::ERROR);
2804+
2805+
ASSERT_EQ(
2806+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2807+
test_controller_1->get_lifecycle_state().id());
2808+
ASSERT_EQ(
2809+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2810+
test_controller_2->get_lifecycle_state().id());
2811+
cm_->update(cm_->now(), rclcpp::Duration::from_seconds(0.01));
2812+
2813+
// Now activate controller 2 and then try to activate controller 1 while deactivating controller 2
2814+
res = cm_->switch_controller(
2815+
{TEST_CONTROLLER_2}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2816+
rclcpp::Duration(0, 0));
2817+
ASSERT_EQ(res, controller_interface::return_type::OK);
2818+
ASSERT_EQ(
2819+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2820+
test_controller_1->get_lifecycle_state().id());
2821+
ASSERT_EQ(
2822+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2823+
test_controller_2->get_lifecycle_state().id());
2824+
cm_->update(cm_->now(), rclcpp::Duration::from_seconds(0.01));
2825+
2826+
res = cm_->switch_controller(
2827+
{TEST_CONTROLLER_1}, {TEST_CONTROLLER_2},
2828+
controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0));
2829+
ASSERT_EQ(res, controller_interface::return_type::OK);
2830+
ASSERT_EQ(
2831+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
2832+
test_controller_1->get_lifecycle_state().id());
2833+
ASSERT_EQ(
2834+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2835+
test_controller_2->get_lifecycle_state().id());
2836+
cm_->update(cm_->now(), rclcpp::Duration::from_seconds(0.01));
2837+
2838+
// deactivate controller 2
2839+
res = cm_->switch_controller(
2840+
{}, {TEST_CONTROLLER_1}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
2841+
rclcpp::Duration(0, 0));
2842+
ASSERT_EQ(res, controller_interface::return_type::OK);
2843+
ASSERT_EQ(
2844+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2845+
test_controller_1->get_lifecycle_state().id());
2846+
ASSERT_EQ(
2847+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
2848+
test_controller_2->get_lifecycle_state().id());
2849+
cm_->update(cm_->now(), rclcpp::Duration::from_seconds(0.01));
2850+
}
2851+
26832852
class TestControllerManagerSrvsWithFailingPerformSwitch : public TestControllerManagerSrvs
26842853
{
26852854
void SetUp()

0 commit comments

Comments
 (0)