add controller chain interfaces usage controller list cache

ros-controls · May 8, 2024 · 93bdaea · 93bdaea
1 parent bd9ca59
commit 93bdaea
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 0 deletions.
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -546,6 +546,9 @@ class ControllerManager : public rclcpp::Node
   std::vector<std::string> activate_command_interface_request_,
     deactivate_command_interface_request_;
 
+  std::map<std::string, std::vector<std::string>> controller_chained_reference_interfaces_cache_;
+  std::map<std::string, std::vector<std::string>> controller_chained_state_interfaces_cache_;
+
   std::string robot_description_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
 

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -819,6 +819,8 @@ controller_interface::return_type ControllerManager::configure_controller(
         controller_chain_spec_[controller_name].following_controllers, ctrl_it->info.name);
       add_element_to_list(
         controller_chain_spec_[ctrl_it->info.name].preceding_controllers, controller_name);
+      add_element_to_list(
+        controller_chained_reference_interfaces_cache_[ctrl_it->info.name], controller_name);
     }
   }
   // This is needed when we start exporting the state interfaces from the controllers
@@ -831,6 +833,8 @@ controller_interface::return_type ControllerManager::configure_controller(
         controller_chain_spec_[controller_name].preceding_controllers, ctrl_it->info.name);
       add_element_to_list(
         controller_chain_spec_[ctrl_it->info.name].following_controllers, controller_name);
+      add_element_to_list(
+        controller_chained_state_interfaces_cache_[ctrl_it->info.name], controller_name);
     }
   }
 
@@ -1439,6 +1443,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
   // initialize the data for the controller chain spec once it is loaded. It is needed, so when we
   // sort the controllers later, they will be added to the list
   controller_chain_spec_[controller.info.name] = ControllerChainSpec();
+  controller_chained_state_interfaces_cache_[controller.info.name] = {};
+  controller_chained_reference_interfaces_cache_[controller.info.name] = {};
 
   executor_->add_node(controller.c->get_node()->get_node_base_interface());
   to.emplace_back(controller);