Merge branch 'master' into add/hardware_component/statistics

Author: saikishor

controller_manager/include/controller_manager/controller_manager.hpp

@@ -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

controller_manager/src/controller_manager.cpp

@@ -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)
@@ -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)
@@ -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
@@ -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
   }
 }
@@ -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
@@ -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(),
@@ -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);
     }
   }
 
@@ -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
   }
 }