From 5df337dc39b747efe4d79bf3bb41bb114804a8ff Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 May 2023 21:57:07 +0200 Subject: [PATCH] remove the controller from the use_references_from_subscribers_ list when needed by the controller to be active --- controller_manager/src/controller_manager.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index db2060a6c2..58d695ce82 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2035,7 +2035,9 @@ void ControllerManager::propagate_deactivation_of_chained_mode( from_chained_mode_request_.push_back(following_ctrl_it->info.name); to_use_references_from_subscribers_.push_back(following_ctrl_it->info.name); RCLCPP_DEBUG( - get_logger(), "Adding controller '%s' in 'from chained mode' request.", + get_logger(), + "Adding controller '%s' in 'from chained mode' and 'use references from " + "subscriber' request.", following_ctrl_it->info.name.c_str()); } } @@ -2159,6 +2161,18 @@ controller_interface::return_type ControllerManager::check_following_controllers "should stay in chained mode.", following_ctrl_it->info.name.c_str()); } + auto ref_from_sub_it = std::find( + to_use_references_from_subscribers_.begin(), to_use_references_from_subscribers_.end(), + following_ctrl_it->info.name); + if (found_it != to_use_references_from_subscribers_.end()) + { + to_use_references_from_subscribers_.erase(ref_from_sub_it); + RCLCPP_DEBUG( + get_logger(), + "Removing controller '%s' in 'use references from subscriber' request because it " + "should stay in chained mode and accept references from the active controller.", + following_ctrl_it->info.name.c_str()); + } } } return controller_interface::return_type::OK;