Skip to content

Commit

Permalink
[CM] Improve memory allocation with buffer variables (#1801)
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor authored Feb 14, 2025
1 parent 254b6e8 commit 3e253fc
Show file tree
Hide file tree
Showing 2 changed files with 95 additions and 96 deletions.
36 changes: 36 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 @@ -604,6 +604,42 @@ class ControllerManager : public rclcpp::Node
};

SwitchParams switch_params_;

struct RTBufferVariables
{
RTBufferVariables()
{
deactivate_controllers_list.reserve(1000);
activate_controllers_using_interfaces_list.reserve(1000);
fallback_controllers_list.reserve(1000);
interfaces_to_start.reserve(1000);
interfaces_to_stop.reserve(1000);
concatenated_string.reserve(5000);
}

const std::string & get_concatenated_string(
const std::vector<std::string> & strings, bool clear_string = true)
{
if (clear_string)
{
concatenated_string.clear();
}
for (const auto & str : strings)
{
concatenated_string.append(str);
concatenated_string.append(" ");
}
return concatenated_string;
}

std::vector<std::string> deactivate_controllers_list;
std::vector<std::string> activate_controllers_using_interfaces_list;
std::vector<std::string> fallback_controllers_list;
std::vector<std::string> interfaces_to_start;
std::vector<std::string> interfaces_to_stop;
std::string concatenated_string;
};
RTBufferVariables rt_buffer_;
};

} // namespace controller_manager
Expand Down
155 changes: 59 additions & 96 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,14 +217,14 @@ void get_active_controllers_using_command_interfaces_of_controller(

void extract_command_interfaces_for_controller(
const controller_manager::ControllerSpec & ctrl,
const hardware_interface::ResourceManager & resource_manager,
const std::unique_ptr<hardware_interface::ResourceManager> & resource_manager,
std::vector<std::string> & request_interface_list)
{
auto command_interface_config = ctrl.c->command_interface_configuration();
std::vector<std::string> command_interface_names = {};
if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
{
command_interface_names = resource_manager.available_command_interfaces();
command_interface_names = resource_manager->available_command_interfaces();
}
if (
command_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL)
Expand All @@ -238,7 +238,7 @@ void extract_command_interfaces_for_controller(
void get_controller_list_command_interfaces(
const std::vector<std::string> & controllers_list,
const std::vector<controller_manager::ControllerSpec> & controllers_spec,
const hardware_interface::ResourceManager & resource_manager,
const std::unique_ptr<hardware_interface::ResourceManager> & resource_manager,
std::vector<std::string> & request_interface_list)
{
for (const auto & controller_name : controllers_list)
Expand Down Expand Up @@ -1459,12 +1459,12 @@ controller_interface::return_type ControllerManager::switch_controller(
if (in_activate_list)
{
extract_command_interfaces_for_controller(
controller, *resource_manager_, activate_command_interface_request_);
controller, resource_manager_, activate_command_interface_request_);
}
if (in_deactivate_list)
{
extract_command_interfaces_for_controller(
controller, *resource_manager_, deactivate_command_interface_request_);
controller, resource_manager_, deactivate_command_interface_request_);
}

// cache mapping between hardware and controllers for stopping when read/write error happens
Expand Down Expand Up @@ -2430,36 +2430,26 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &

if (!ok)
{
std::vector<std::string> stop_request = {};
std::string failed_hardware_string;
failed_hardware_string.reserve(500);
rt_buffer_.deactivate_controllers_list.clear();
// Determine controllers to stop
for (const auto & hardware_name : failed_hardware_names)
{
failed_hardware_string.append(hardware_name);
failed_hardware_string.append(" ");
auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
stop_request.insert(stop_request.end(), controllers.begin(), controllers.end());
}
std::string stop_request_string;
stop_request_string.reserve(500);
for (const auto & controller : stop_request)
{
stop_request_string.append(controller);
stop_request_string.append(" ");
rt_buffer_.deactivate_controllers_list.insert(
rt_buffer_.deactivate_controllers_list.end(), controllers.begin(), controllers.end());
}
RCLCPP_ERROR(
get_logger(),
"Deactivating following hardware components as their read cycle resulted in an error: [ %s]",
failed_hardware_string.c_str());
rt_buffer_.get_concatenated_string(failed_hardware_names).c_str());
RCLCPP_ERROR_EXPRESSION(
get_logger(), !stop_request_string.empty(),
get_logger(), !rt_buffer_.deactivate_controllers_list.empty(),
"Deactivating following controllers as their hardware components read cycle resulted in an "
"error: [ %s]",
stop_request_string.c_str());
rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str());
std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();
deactivate_controllers(rt_controller_list, stop_request);
deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
// TODO(destogl): do auto-start of broadcasters
}
}
Expand Down Expand Up @@ -2532,7 +2522,7 @@ controller_interface::return_type ControllerManager::update(
"configuration (use_sim_time parameter) and if a valid clock source is available");
}

std::vector<std::string> failed_controllers_list;
rt_buffer_.deactivate_controllers_list.clear();
for (const auto & loaded_controller : rt_controller_list)
{
// TODO(v-lopez) we could cache this information
Expand Down Expand Up @@ -2630,21 +2620,18 @@ controller_interface::return_type ControllerManager::update(

if (controller_ret != controller_interface::return_type::OK)
{
failed_controllers_list.push_back(loaded_controller.info.name);
rt_buffer_.deactivate_controllers_list.push_back(loaded_controller.info.name);
ret = controller_ret;
}
}
}
}
if (!failed_controllers_list.empty())
if (!rt_buffer_.deactivate_controllers_list.empty())
{
const auto FALLBACK_STACK_MAX_SIZE = 500;
std::vector<std::string> active_controllers_using_interfaces(failed_controllers_list);
active_controllers_using_interfaces.reserve(FALLBACK_STACK_MAX_SIZE);
std::vector<std::string> cumulative_fallback_controllers;
cumulative_fallback_controllers.reserve(FALLBACK_STACK_MAX_SIZE);
rt_buffer_.fallback_controllers_list.clear();
rt_buffer_.activate_controllers_using_interfaces_list.clear();

for (const auto & failed_ctrl : failed_controllers_list)
for (const auto & failed_ctrl : rt_buffer_.deactivate_controllers_list)
{
auto ctrl_it = std::find_if(
rt_controller_list.begin(), rt_controller_list.end(),
Expand All @@ -2653,72 +2640,58 @@ controller_interface::return_type ControllerManager::update(
{
for (const auto & fallback_controller : ctrl_it->info.fallback_controllers_names)
{
cumulative_fallback_controllers.push_back(fallback_controller);
rt_buffer_.fallback_controllers_list.push_back(fallback_controller);
get_active_controllers_using_command_interfaces_of_controller(
fallback_controller, rt_controller_list, active_controllers_using_interfaces);
fallback_controller, rt_controller_list,
rt_buffer_.activate_controllers_using_interfaces_list);
}
}
}
std::string controllers_string;
controllers_string.reserve(500);
for (const auto & controller : failed_controllers_list)
{
controllers_string.append(controller);
controllers_string.append(" ");
}

RCLCPP_ERROR(
get_logger(), "Deactivating controllers : [ %s] as their update resulted in an error!",
controllers_string.c_str());
if (active_controllers_using_interfaces.size() > failed_controllers_list.size())
{
controllers_string.clear();
for (size_t i = failed_controllers_list.size();
i < active_controllers_using_interfaces.size(); i++)
{
controllers_string.append(active_controllers_using_interfaces[i]);
controllers_string.append(" ");
}
RCLCPP_ERROR_EXPRESSION(
get_logger(), !controllers_string.empty(),
"Deactivating controllers : [ %s] using the command interfaces needed for the fallback "
"controllers to activate.",
controllers_string.c_str());
}
if (!cumulative_fallback_controllers.empty())
{
controllers_string.clear();
for (const auto & controller : cumulative_fallback_controllers)
{
controllers_string.append(controller);
controllers_string.append(" ");
}
RCLCPP_ERROR(
get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str());
}
std::vector<std::string> failed_controller_interfaces, fallback_controller_interfaces;
failed_controller_interfaces.reserve(500);
rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str());
RCLCPP_ERROR_EXPRESSION(
get_logger(), !rt_buffer_.activate_controllers_using_interfaces_list.empty(),
"Deactivating controllers : [ %s] using the command interfaces needed for the fallback "
"controllers to activate.",
rt_buffer_.get_concatenated_string(rt_buffer_.activate_controllers_using_interfaces_list)
.c_str());
RCLCPP_ERROR_EXPRESSION(
get_logger(), !rt_buffer_.fallback_controllers_list.empty(),
"Activating fallback controllers : [ %s]",
rt_buffer_.get_concatenated_string(rt_buffer_.fallback_controllers_list).c_str());
std::for_each(
rt_buffer_.activate_controllers_using_interfaces_list.begin(),
rt_buffer_.activate_controllers_using_interfaces_list.end(),
[this](const std::string & controller)
{ add_element_to_list(rt_buffer_.deactivate_controllers_list, controller); });

// Retrieve the interfaces to start and stop from the hardware end
rt_buffer_.interfaces_to_start.clear();
rt_buffer_.interfaces_to_stop.clear();
get_controller_list_command_interfaces(
failed_controllers_list, rt_controller_list, *resource_manager_,
failed_controller_interfaces);
rt_buffer_.deactivate_controllers_list, rt_controller_list, resource_manager_,
rt_buffer_.interfaces_to_stop);
get_controller_list_command_interfaces(
cumulative_fallback_controllers, rt_controller_list, *resource_manager_,
fallback_controller_interfaces);
if (!failed_controller_interfaces.empty())
rt_buffer_.fallback_controllers_list, rt_controller_list, resource_manager_,
rt_buffer_.interfaces_to_start);
if (!rt_buffer_.interfaces_to_stop.empty() || !rt_buffer_.interfaces_to_start.empty())
{
if (!(resource_manager_->prepare_command_mode_switch(
fallback_controller_interfaces, failed_controller_interfaces) &&
rt_buffer_.interfaces_to_start, rt_buffer_.interfaces_to_stop) &&
resource_manager_->perform_command_mode_switch(
fallback_controller_interfaces, failed_controller_interfaces)))
rt_buffer_.interfaces_to_start, rt_buffer_.interfaces_to_stop)))
{
RCLCPP_ERROR(
get_logger(),
"Error while attempting mode switch when deactivating controllers in update cycle!");
}
}
deactivate_controllers(rt_controller_list, active_controllers_using_interfaces);
if (!cumulative_fallback_controllers.empty())
deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
if (!rt_buffer_.fallback_controllers_list.empty())
{
activate_controllers(rt_controller_list, cumulative_fallback_controllers);
activate_controllers(rt_controller_list, rt_buffer_.fallback_controllers_list);
}
}

Expand All @@ -2739,37 +2712,27 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration

if (!ok)
{
std::vector<std::string> stop_request = {};
std::string failed_hardware_string;
failed_hardware_string.reserve(500);
rt_buffer_.deactivate_controllers_list.clear();
// Determine controllers to stop
for (const auto & hardware_name : failed_hardware_names)
{
failed_hardware_string.append(hardware_name);
failed_hardware_string.append(" ");
auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
stop_request.insert(stop_request.end(), controllers.begin(), controllers.end());
}
std::string stop_request_string;
stop_request_string.reserve(500);
for (const auto & controller : stop_request)
{
stop_request_string.append(controller);
stop_request_string.append(" ");
rt_buffer_.deactivate_controllers_list.insert(
rt_buffer_.deactivate_controllers_list.end(), controllers.begin(), controllers.end());
}
RCLCPP_ERROR(
get_logger(),
"Deactivating following hardware components as their write cycle resulted in an error: [ "
"%s]",
failed_hardware_string.c_str());
rt_buffer_.get_concatenated_string(failed_hardware_names).c_str());
RCLCPP_ERROR_EXPRESSION(
get_logger(), !stop_request_string.empty(),
get_logger(), !rt_buffer_.deactivate_controllers_list.empty(),
"Deactivating following controllers as their hardware components write cycle resulted in an "
"error: [ %s]",
stop_request_string.c_str());
rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str());
std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();
deactivate_controllers(rt_controller_list, stop_request);
deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
// TODO(destogl): do auto-start of broadcasters
}
}
Expand Down

0 comments on commit 3e253fc

Please sign in to comment.