Merge branch 'master' into feat/abs_pos_and_torque_interfaces

Author: saikishor

controller_manager/doc/userdoc.rst

@@ -79,11 +79,6 @@ robot_description [std_msgs::msg::String]
 Parameters
 -----------
 
-enforce_command_limits (optional; bool; default: ``false`` for Jazzy and earlier distro and ``true`` for Rolling and newer distros)
-  Enforces the joint limits to the joint command interfaces.
-  If the command is outside the limits, the command is clamped to be within the limits depending on the type of configured joint limits in the URDF.
-  If the command is within the limits, the command is passed through without any changes.
-
 <controller_name>.type
   Name of a plugin exported using ``pluginlib`` for a controller.
   This is a class from which controller's instance with name "``controller_name``" is created.

controller_manager/include/controller_manager/controller_manager.hpp

@@ -673,7 +673,6 @@ class ControllerManager : public rclcpp::Node
   std::string robot_description_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
   rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
-  bool enforce_command_limits_;
 
   controller_manager::MovingAverageStatistics periodicity_stats_;
 

controller_manager/src/controller_manager.cpp

@@ -490,7 +490,7 @@ void ControllerManager::init_controller_manager()
   // Get parameters needed for RT "update" loop to work
   if (is_resource_manager_initialized())
   {
-    if (enforce_command_limits_)
+    if (params_->enforce_command_limits)
     {
       resource_manager_->import_joint_limiters(robot_description_);
     }
@@ -556,12 +556,8 @@ void ControllerManager::init_controller_manager()
         RCLCPP_INFO(get_logger(), "Shutting down the controller manager.");
       }));
 
-  // Declare the enforce_command_limits parameter such a way that it is enabled by default for
-  // rolling and newer alone
-  enforce_command_limits_ =
-    this->get_parameter_or("enforce_command_limits", RCLCPP_VERSION_MAJOR >= 29 ? true : false);
   RCLCPP_INFO_EXPRESSION(
-    get_logger(), enforce_command_limits_, "Enforcing command limits is enabled...");
+    get_logger(), params_->enforce_command_limits, "Enforcing command limits is enabled...");
 }
 
 void ControllerManager::initialize_parameters()
@@ -615,7 +611,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
 
 void ControllerManager::init_resource_manager(const std::string & robot_description)
 {
-  if (enforce_command_limits_)
+  if (params_->enforce_command_limits)
   {
     resource_manager_->import_joint_limiters(robot_description_);
   }

controller_manager/src/controller_manager_parameters.yaml

@@ -6,6 +6,13 @@ controller_manager:
     description: "The frequency of controller manager's real-time update loop. This loop reads states from hardware, updates controllers and writes commands to hardware."
   }
 
+  enforce_command_limits: {
+    type: bool,
+    default_value: true,
+    read_only: true,
+    description: "If true, the controller manager will enforce command limits defined in the robot description. If false, no limits will be enforced. If true, when the command is outside the limits, the command is clamped to be within the limits depending on the type of configured joint limits defined in the robot description. If the command is within the limits, the command is passed through without any changes.",
+  }
+
   hardware_components_initial_state:
     unconfigured: {
       type: string_array,
@@ -28,6 +35,7 @@ controller_manager:
     shutdown_on_initial_state_failure: {
       type: bool,
       default_value: false,
+      read_only: true,
       description: "Specifies whether the controller manager should shut down if setting the desired initial state fails during startup.",
     }
 

hardware_interface/include/hardware_interface/types/statistics_types.hpp

@@ -22,7 +22,7 @@
 
 #include "libstatistics_collector/moving_average_statistics/moving_average.hpp"
 #include "libstatistics_collector/moving_average_statistics/types.hpp"
-#ifndef _WIN32
+#if !defined(_WIN32) && !defined(__APPLE__)
 #include "realtime_tools/mutex.hpp"
 #define DEFAULT_MUTEX realtime_tools::prio_inherit_mutex
 #else

hardware_interface/src/resource_manager.cpp

@@ -91,18 +91,32 @@ std::string interfaces_to_string(
   return ss.str();
 };
 
-void get_hardware_related_interfaces(
+void find_common_hardware_interfaces(
   const std::vector<std::string> & hw_command_itfs,
   const std::vector<std::string> & start_stop_interfaces_list,
   std::vector<std::string> & hw_interfaces)
 {
   hw_interfaces.clear();
-  for (const auto & interface : start_stop_interfaces_list)
+
+  // decide which input vector is shorter.
+  const auto & shorter_vec = hw_command_itfs.size() < start_stop_interfaces_list.size()
+                               ? hw_command_itfs
+                               : start_stop_interfaces_list;
+  const auto & longer_vec =
+    &shorter_vec == &hw_command_itfs ? start_stop_interfaces_list : hw_command_itfs;
+
+  // reserve exactly the worst-case result size (all of the smaller one).
+  hw_interfaces.reserve(shorter_vec.size());
+
+  // build a hash set from the smaller vector.
+  std::unordered_set<std::string> lookup(shorter_vec.begin(), shorter_vec.end());
+
+  // iterate through the larger vector; test membership in constant time.
+  for (const auto & name : longer_vec)
   {
-    if (
-      std::find(hw_command_itfs.begin(), hw_command_itfs.end(), interface) != hw_command_itfs.end())
+    if (lookup.find(name) != lookup.end())
     {
-      hw_interfaces.push_back(interface);
+      hw_interfaces.push_back(name);
     }
   }
 }
@@ -1933,8 +1947,8 @@ bool ResourceManager::prepare_command_mode_switch(
     for (auto & component : components)
     {
       const auto & hw_command_itfs = hardware_info_map.at(component.get_name()).command_interfaces;
-      get_hardware_related_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer);
-      get_hardware_related_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer);
+      find_common_hardware_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer);
+      find_common_hardware_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer);
       if (start_interfaces_buffer.empty() && stop_interfaces_buffer.empty())
       {
         RCLCPP_DEBUG(
@@ -2023,8 +2037,8 @@ bool ResourceManager::perform_command_mode_switch(
     for (auto & component : components)
     {
       const auto & hw_command_itfs = hardware_info_map.at(component.get_name()).command_interfaces;
-      get_hardware_related_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer);
-      get_hardware_related_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer);
+      find_common_hardware_interfaces(hw_command_itfs, start_interfaces, start_interfaces_buffer);
+      find_common_hardware_interfaces(hw_command_itfs, stop_interfaces, stop_interfaces_buffer);
       if (start_interfaces_buffer.empty() && stop_interfaces_buffer.empty())
       {
         RCLCPP_DEBUG(