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

Jump to bottom

Fix 1638 backport conflicts #1663

Merged
merged 1 commit into from
Aug 11, 2024
Merged

Fix 1638 backport conflicts #1663

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
81 changes: 9 additions & 72 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -841,21 +841,8 @@ controller_interface::return_type ControllerManager::switch_controller(
const std::vector<std::string> & deactivate_controllers, int strictness, bool activate_asap,
const rclcpp::Duration & timeout)
{
<<<<<<< HEAD
switch_params_ = SwitchParams();
=======
if (!is_resource_manager_initialized())
{
RCLCPP_ERROR(
get_logger(),
"Resource Manager is not initialized yet! Please provide robot description on "
"'robot_description' topic before trying to switch controllers.");
return controller_interface::return_type::ERROR;
}

// reset the switch param internal variables
switch_params_.reset();
>>>>>>> a1ad523 (refactor SwitchParams to fix the incosistencies in the spawner tests (#1638))

if (!deactivate_request_.empty() || !activate_request_.empty())
{
Expand Down Expand Up @@ -1329,6 +1316,12 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co

void ControllerManager::manage_switch()
{
std::unique_lock<std::mutex> guard(switch_params_.mutex, std::try_to_lock);
if (!guard.owns_lock())
{
RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle.");
return;
}
// Ask hardware interfaces to change mode
if (!resource_manager_->perform_command_mode_switch(
activate_command_interface_request_, deactivate_command_interface_request_))
Expand All @@ -1353,6 +1346,9 @@ void ControllerManager::manage_switch()
}

// TODO(destogl): move here "do_switch = false"

switch_params_.do_switch = false;
switch_params_.cv.notify_all();
}

void ControllerManager::deactivate_controllers()
Expand Down Expand Up @@ -2022,66 +2018,7 @@ std::vector<std::string> ControllerManager::get_controller_names()

void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & period)
{
<<<<<<< HEAD
resource_manager_->read(time, period);
=======
auto [ok, failed_hardware_names] = resource_manager_->read(time, period);

if (!ok)
{
std::vector<std::string> stop_request = {};
// Determine controllers to stop
for (const auto & hardware_name : failed_hardware_names)
{
auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name);
stop_request.insert(stop_request.end(), controllers.begin(), controllers.end());
}

std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();
deactivate_controllers(rt_controller_list, stop_request);
// TODO(destogl): do auto-start of broadcasters
}
}

void ControllerManager::manage_switch()
{
std::unique_lock<std::mutex> guard(switch_params_.mutex, std::try_to_lock);
if (!guard.owns_lock())
{
RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle.");
return;
}
// Ask hardware interfaces to change mode
if (!resource_manager_->perform_command_mode_switch(
activate_command_interface_request_, deactivate_command_interface_request_))
{
RCLCPP_ERROR(get_logger(), "Error while performing mode switch.");
}

std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();

deactivate_controllers(rt_controller_list, deactivate_request_);

switch_chained_mode(to_chained_mode_request_, true);
switch_chained_mode(from_chained_mode_request_, false);

// activate controllers once the switch is fully complete
if (!switch_params_.activate_asap)
{
activate_controllers(rt_controller_list, activate_request_);
}
else
{
// activate controllers as soon as their required joints are done switching
activate_controllers_asap(rt_controller_list, activate_request_);
}

// All controllers switched --> switching done
switch_params_.do_switch = false;
switch_params_.cv.notify_all();
>>>>>>> a1ad523 (refactor SwitchParams to fix the incosistencies in the spawner tests (#1638))
}

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