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

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
Open

[CM] Improve memory allocation with buffer variables #1801

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
b69f1ca
add initial version
saikishor Oct 16, 2024
6f8f068
compilation fixes
saikishor Oct 17, 2024
7b6c6ab
update the RTBufferVariables and reserve the variables
saikishor Oct 17, 2024
ea7afb3
Use the rt_buffer inside the read cycle
saikishor Oct 17, 2024
182e43e
Use the rt_buffer inside the write cycle
saikishor Oct 17, 2024
200918b
Cleanup variables in the RTBufferVariables
saikishor Oct 17, 2024
58386ae
use the rt_buffer variables in the update method for realtimeness
saikishor Oct 17, 2024
636fdd0
Merge branch 'master' into improve/memory_allocation/buffer_variables
saikishor Jan 1, 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
32 changes: 32 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 @@ -590,6 +590,38 @@ 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);
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::string concatenated_string;
};
RTBufferVariables rt_buffer_;
};

} // namespace controller_manager
Expand Down
125 changes: 43 additions & 82 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2327,36 +2327,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 @@ -2429,7 +2419,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 @@ -2527,21 +2517,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 @@ -2550,52 +2537,36 @@ 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());
}
deactivate_controllers(rt_controller_list, active_controllers_using_interfaces);
if (!cumulative_fallback_controllers.empty())
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); });
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 @@ -2614,37 +2585,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
Loading