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

[CM] Improve memory allocation with buffer variables #1801

Merged
merged 9 commits into from
Feb 14, 2025
+95 −96
Merged

[CM] Improve memory allocation with buffer variables #1801

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
5948ee9
add initial version
saikishor Oct 16, 2024
1068c4e
compilation fixes
saikishor Oct 17, 2024
06a63d1
update the RTBufferVariables and reserve the variables
saikishor Oct 17, 2024
a280a0d
Use the rt_buffer inside the read cycle
saikishor Oct 17, 2024
b3d0edc
Use the rt_buffer inside the write cycle
saikishor Oct 17, 2024
0faf1a9
Cleanup variables in the RTBufferVariables
saikishor Oct 17, 2024
52a1159
use the rt_buffer variables in the update method for realtimeness
saikishor Oct 17, 2024
4f8806a
change the new variables for switching interfaces to RTBuffer variables
saikishor Feb 13, 2025
74dcce2
Use resource manager smart pointer instead of reference
saikishor Feb 13, 2025
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
36 changes: 36 additions & 0 deletions controller_manager/include/controller_manager/controller_manager.hpp
View file
Open in desktop
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
Open in desktop
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