Let get_ordered_interfaces throw if input vector size does not fit (#2528)

Author: christophfroehlich

controller_interface/include/controller_interface/helpers.hpp

@@ -17,6 +17,8 @@
 
 #include <algorithm>
 #include <functional>
+#include <memory>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -37,15 +39,24 @@ namespace controller_interface
  *  If joint names are used for ordering, \p interface_type specifies valid interface.
  *  If full interface names are used for ordering, \p interface_type should be empty string ("").
  * \param[in] interface_type used for ordering interfaces with respect to joint names.
- * \param[out] ordered_interfaces vector with ordered interfaces.
+ * \param[out] ordered_interfaces vector with ordered interfaces. Has to have the same capacity as
+ * \p ordered_names size. Throws otherwise.
+ * \throws std::range_error if the capacity of ordered_interfaces is less than the size of
+ * ordered_names.
  * \return true if all interfaces or joints in \p ordered_names are found, otherwise false.
  */
 template <typename T>
 bool get_ordered_interfaces(
   std::vector<T> & unordered_interfaces, const std::vector<std::string> & ordered_names,
   const std::string & interface_type, std::vector<std::reference_wrapper<T>> & ordered_interfaces)
 {
-  ordered_interfaces.reserve(ordered_names.size());
+  if (ordered_interfaces.capacity() < ordered_names.size())
+  {
+    throw std::range_error(
+      "Capacity of ordered_interfaces (" + std::to_string(ordered_interfaces.capacity()) +
+      ") has to be equal or higher as size of ordered_names (" +
+      std::to_string(ordered_names.size()) + ") for realtime reasons.");
+  }
   for (const auto & name : ordered_names)
   {
     for (auto & interface : unordered_interfaces)

controller_interface/include/semantic_components/gps_sensor.hpp

@@ -52,6 +52,7 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
       interface_names_.emplace_back(name + "/" + "latitude_covariance");
       interface_names_.emplace_back(name + "/" + "longitude_covariance");
       interface_names_.emplace_back(name + "/" + "altitude_covariance");
+      state_interfaces_.reserve(state_interfaces_.capacity() + 3);
     }
   }
 

controller_interface/include/semantic_components/semantic_component_interface.hpp

@@ -104,6 +104,10 @@ class SemanticComponentInterface
    */
   bool get_values_as_message(MessageReturnType & /* message */) { return false; }
 
+  // delete copy constructor, because
+  // copy will change capacity of member variables
+  SemanticComponentInterface(const SemanticComponentInterface &) = delete;
+
 protected:
   std::string name_;
   std::vector<std::string> interface_names_;

doc/migration.rst

@@ -5,6 +5,10 @@ Migration Guides: Jazzy to Kilted
 
 This list summarizes important changes between Jazzy (previous) and Kilted (current) releases, where changes to user code might be necessary.
 
+controller_interface
+********************
+* ``get_ordered_interfaces`` now throws if the size of the output vector does not match the size of the input names vector (`#2528 <https://github.com/ros-controls/ros2_control/pull/2528>`__).
+
 hardware_interface
 ******************