Skip to content

Commit

Permalink
move the logic to a separate function
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor committed Jun 25, 2023
1 parent 84f7b1d commit 5a067f7
Show file tree
Hide file tree
Showing 2 changed files with 66 additions and 53 deletions.
20 changes: 20 additions & 0 deletions controller_manager/include/controller_manager/controller_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -389,6 +389,26 @@ class ControllerManager : public rclcpp::Node
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it);

/// A functor to be used in the std::sort method inorder to sort the controllers to be able to
/// execute then in a proper order
/**
* @brief controller_sorting - A functor that compares the controllers ctrl_a and ctrl_b and then
* return which comes first in the sequence
*
* @note The following conditions needs to be handled while ordering the controller list
* 1. The controllers that do not use any state or command interfaces are updated first
* 2. The controllers that use only the state system interfaces only are updated next
* 3. The controllers that use any of an another controller's reference interface are updated
* before the preceding controller
* 4. The controllers that use the controller's estimated interfaces are updated after the
* preceding controller
* 5. The controllers that only use the system's command interfaces are updated last
* 6. All inactive controllers go at the end of the list
*
* @return true, if ctrl_a needs to execute first, else false
*/
bool controller_sorting(const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b);

void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
diagnostic_updater::Updater diagnostics_updater_;

Expand Down
99 changes: 46 additions & 53 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1036,59 +1036,7 @@ controller_interface::return_type ControllerManager::switch_controller(
}

// Reordering the controllers
/// @note The following conditions needs to be handled while ordering the controller list
/// 1. The controllers that do not use any state or command interfaces are updated first
/// 2. The controllers that use only the state system interfaces only are updated next
/// 3. The controllers that use any of an another controller's reference interface are updated
/// before the preceding controller
/// 4. The controllers that use the controller's estimated interfaces are updated after the
/// preceding controller
/// 5. The controllers that only use the system's command interfaces are updated last
/// 6. All inactive controllers go at the end of the list
auto sorting_lambda = [](ControllerSpec ctrl_a, ControllerSpec ctrl_b)
{
// If the controllers are not active, then go to the end of the list
if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c)) 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())
{
// The case of the controllers that don't have any command interfaces. For instance,
// joint_state_broadcaster
return true;
}
else
{
if (ctrl_b.c->is_chainable())
{
// If the ctrl_a's command interface is the one exported by the ctrl_b then ctrl_a should be
// infront of ctrl_b
auto it = std::find_if(
cmd_itfs.begin(), cmd_itfs.end(),
[ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); });
if (it != cmd_itfs.end())
{
return true;
}
else
{
// If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be
// infront of ctrl_a
auto state_it = std::find_if(
cmd_itfs.begin(), cmd_itfs.end(),
[ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); });
if (it != cmd_itfs.end())
{
return false;
}
}
}
// The rest of the cases, basically end up at the end of the list
return false;
}
};
std::sort(to.begin(), to.end(), sorting_lambda);
std::sort(to.begin(), to.end(), &ControllerManager::controller_sorting);

// switch lists
rt_controllers_wrapper_.switch_updated_list(guard);
Expand Down Expand Up @@ -2248,6 +2196,51 @@ controller_interface::return_type ControllerManager::check_preceeding_controller
}
}
return controller_interface::return_type::OK;
}

bool ControllerManager::controller_sorting(
const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b)
{
// If the controllers are not active, then go to the end of the list
if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c)) 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())
{
// The case of the controllers that don't have any command interfaces. For instance,
// joint_state_broadcaster
return true;
}
else
{
if (ctrl_b.c->is_chainable())
{
// If the ctrl_a's command interface is the one exported by the ctrl_b then ctrl_a should be
// infront of ctrl_b
auto it = std::find_if(
cmd_itfs.begin(), cmd_itfs.end(),
[ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); });
if (it != cmd_itfs.end())
{
return true;
}
else
{
// If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be
// infront of ctrl_a
auto state_it = std::find_if(
cmd_itfs.begin(), cmd_itfs.end(),
[ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); });
if (it != cmd_itfs.end())
{
return false;
}
}
}
// The rest of the cases, basically end up at the end of the list
return false;
}
};

void ControllerManager::controller_activity_diagnostic_callback(
Expand Down

0 comments on commit 5a067f7

Please sign in to comment.