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.

Sign up for GitHub

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 sorting bug when the interfaces are not configured (fixes #1164) #1165

Merged
merged 2 commits into from
Nov 14, 2023
Merged

Fix the controller sorting bug when the interfaces are not configured (fixes #1164) #1165

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1d685a3
throw exception trying to access interfaces in unconfigured state
saikishor Nov 13, 2023
155969d
Only access the interface configuration if the controller is configur…
saikishor Nov 13, 2023
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
13 changes: 13 additions & 0 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,9 @@ std::vector<std::string> get_following_controller_names(

return following_controllers;
}
// If the controller is not configured, return empty
if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c)))
return following_controllers;
const auto cmd_itfs = controller_it->c->command_interface_configuration().names;
for (const auto & itf : cmd_itfs)
{
Expand Down Expand Up @@ -206,6 +209,8 @@ std::vector<std::string> get_preceding_controller_names(
}
for (const auto & ctrl : controllers)
{
// If the controller is not configured, then continue
if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) continue;
auto cmd_itfs = ctrl.c->command_interface_configuration().names;
for (const auto & itf : cmd_itfs)
{
Expand Down Expand Up @@ -2436,6 +2441,14 @@ bool ControllerManager::controller_sorting(
const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b,
const std::vector<controller_manager::ControllerSpec> & controllers)
{
// If the neither of the controllers are configured, then return false
if (!((is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) &&
(is_controller_active(ctrl_b.c) || is_controller_inactive(ctrl_b.c))))
{
if (is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) return true;
return false;
}

const std::vector<std::string> cmd_itfs = ctrl_a.c->command_interface_configuration().names;
const std::vector<std::string> state_itfs = ctrl_a.c->state_interface_configuration().names;
if (cmd_itfs.empty() || !ctrl_a.c->is_chainable())
Expand Down
24 changes: 22 additions & 2 deletions controller_manager/test/test_chainable_controller/test_chainable_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,13 +32,33 @@ TestChainableController::TestChainableController()
controller_interface::InterfaceConfiguration
TestChainableController::command_interface_configuration() const
{
return cmd_iface_cfg_;
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return cmd_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get command interface configuration until the controller is configured.");
}
}

controller_interface::InterfaceConfiguration
TestChainableController::state_interface_configuration() const
{
return state_iface_cfg_;
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return state_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get state interface configuration until the controller is configured.");
}
}

controller_interface::return_type TestChainableController::update_reference_from_subscribers(
Expand Down
24 changes: 22 additions & 2 deletions controller_manager/test/test_controller/test_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,12 +31,32 @@ TestController::TestController()

controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
{
return cmd_iface_cfg_;
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return cmd_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get command interface configuration until the controller is configured.");
}
}

controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
{
return state_iface_cfg_;
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return state_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get state interface configuration until the controller is configured.");
}
}

controller_interface::return_type TestController::update(
Expand Down
Loading