Semantic components cleanup

controller_interface/include/semantic_components/force_torque_sensor.hpp

@@ -25,26 +25,23 @@
 
 namespace semantic_components
 {
+constexpr std::size_t FORCES_SIZE = 3;
 class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::Wrench>
 {
 public:
   /// Constructor for "standard" 6D FTS
-  explicit ForceTorqueSensor(const std::string & name) : SemanticComponentInterface(name, 6)
+  explicit ForceTorqueSensor(const std::string & name)
+  : SemanticComponentInterface(
+      name,
+      // If 6D FTS use standard names
+      {{name + "/" + "force.x"},
+       {name + "/" + "force.y"},
+       {name + "/" + "force.z"},
+       {name + "/" + "torque.x"},
+       {name + "/" + "torque.y"},
+       {name + "/" + "torque.z"}}),
+    existing_axes_({{true, true, true, true, true, true}})
   {
-    // If 6D FTS use standard names
-    interface_names_.emplace_back(name_ + "/" + "force.x");
-    interface_names_.emplace_back(name_ + "/" + "force.y");
-    interface_names_.emplace_back(name_ + "/" + "force.z");
-    interface_names_.emplace_back(name_ + "/" + "torque.x");
-    interface_names_.emplace_back(name_ + "/" + "torque.y");
-    interface_names_.emplace_back(name_ + "/" + "torque.z");
-
-    // Set all interfaces existing
-    std::fill(existing_axes_.begin(), existing_axes_.end(), true);
-
-    // Set default force and torque values to NaN
-    std::fill(forces_.begin(), forces_.end(), std::numeric_limits<double>::quiet_NaN());
-    std::fill(torques_.begin(), torques_.end(), std::numeric_limits<double>::quiet_NaN());
   }
 
   /// Constructor for 6D FTS with custom interface names.
@@ -62,7 +59,8 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
     const std::string & interface_torque_y, const std::string & interface_torque_z)
   : SemanticComponentInterface("", 6)
   {
-    auto check_and_add_interface = [this](const std::string & interface_name, const size_t index)
+    auto check_and_add_interface =
+      [this](const std::string & interface_name, const std::size_t index)
     {
       if (!interface_name.empty())
       {
@@ -81,56 +79,55 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
     check_and_add_interface(interface_torque_x, 3);
     check_and_add_interface(interface_torque_y, 4);
     check_and_add_interface(interface_torque_z, 5);
-
-    // Set default force and torque values to NaN
-    std::fill(forces_.begin(), forces_.end(), std::numeric_limits<double>::quiet_NaN());
-    std::fill(torques_.begin(), torques_.end(), std::numeric_limits<double>::quiet_NaN());
   }
 
-  virtual ~ForceTorqueSensor() = default;
-
   /// Return forces.
   /**
    * Return forces of a FTS.
    *
-   * \return array of size 3 with force values.
+   * \return array of size 3 with force values (x, y, z).
    */
-  std::array<double, 3> get_forces()
+  std::array<double, 3> get_forces() const
   {
-    size_t interface_counter = 0;
-    for (size_t i = 0; i < 3; ++i)
+    std::array<double, 3> forces;
+    forces.fill(std::numeric_limits<double>::quiet_NaN());
+    std::size_t interface_counter{0};
+    for (auto i = 0u; i < forces.size(); ++i)
     {
       if (existing_axes_[i])
       {
-        forces_[i] = state_interfaces_[interface_counter].get().get_value();
+        forces[i] = state_interfaces_[interface_counter].get().get_value();
         ++interface_counter;
       }
     }
-    return forces_;
+    return forces;
   }
 
   /// Return torque.
   /**
    * Return torques of a FTS.
    *
-   * \return array of size 3 with torque values.
+   * \return array of size 3 with torque values (x, y, z).
    */
-  std::array<double, 3> get_torques()
+  std::array<double, 3> get_torques() const
   {
+    std::array<double, 3> torques;
+    torques.fill(std::numeric_limits<double>::quiet_NaN());
+
     // find out how many force interfaces are being used
     // torque interfaces will be found from the next index onward
-    auto torque_interface_counter =
-      static_cast<size_t>(std::count(existing_axes_.begin(), existing_axes_.begin() + 3, true));
+    auto torque_interface_counter = static_cast<std::size_t>(
+      std::count(existing_axes_.begin(), existing_axes_.begin() + FORCES_SIZE, true));
 
-    for (size_t i = 3; i < 6; ++i)
+    for (auto i = 0u; i < torques.size(); ++i)
     {
-      if (existing_axes_[i])
+      if (existing_axes_[i + FORCES_SIZE])
       {
-        torques_[i - 3] = state_interfaces_[torque_interface_counter].get().get_value();
+        torques[i] = state_interfaces_[torque_interface_counter].get().get_value();
         ++torque_interface_counter;
       }
     }
-    return torques_;
+    return torques;
   }
 
   /// Return Wrench message with forces and torques.
@@ -141,29 +138,24 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
    *
    * \return wrench message from values;
    */
-  bool get_values_as_message(geometry_msgs::msg::Wrench & message)
+  bool get_values_as_message(geometry_msgs::msg::Wrench & message) const
   {
-    // call get_forces() and get_troque() to update with the latest values
-    get_forces();
-    get_torques();
-
-    // update the message values
-    message.force.x = forces_[0];
-    message.force.y = forces_[1];
-    message.force.z = forces_[2];
-    message.torque.x = torques_[0];
-    message.torque.y = torques_[1];
-    message.torque.z = torques_[2];
+    const auto [force_x, force_y, force_z] = get_forces();
+    const auto [torque_x, torque_y, torque_z] = get_torques();
+
+    message.force.x = force_x;
+    message.force.y = force_y;
+    message.force.z = force_z;
+    message.torque.x = torque_x;
+    message.torque.y = torque_y;
+    message.torque.z = torque_z;
 
     return true;
   }
 
 protected:
   /// Vector with existing axes for sensors with less then 6D axes.
-  // Order is: force X, force Y, force Z, torque X, torque Y, torque Z.
   std::array<bool, 6> existing_axes_;
-  std::array<double, 3> forces_;
-  std::array<double, 3> torques_;
 };
 
 }  // namespace semantic_components

controller_interface/include/semantic_components/imu_sensor.hpp

@@ -27,110 +27,98 @@ namespace semantic_components
 class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
 {
 public:
-  explicit IMUSensor(const std::string & name) : SemanticComponentInterface(name, 10)
+  explicit IMUSensor(const std::string & name)
+  : SemanticComponentInterface(
+      name, {{name + "/" + "orientation.x"},
+             {name + "/" + "orientation.y"},
+             {name + "/" + "orientation.z"},
+             {name + "/" + "orientation.w"},
+             {name + "/" + "angular_velocity.x"},
+             {name + "/" + "angular_velocity.y"},
+             {name + "/" + "angular_velocity.z"},
+             {name + "/" + "linear_acceleration.x"},
+             {name + "/" + "linear_acceleration.y"},
+             {name + "/" + "linear_acceleration.z"}})
   {
-    interface_names_.emplace_back(name_ + "/" + "orientation.x");
-    interface_names_.emplace_back(name_ + "/" + "orientation.y");
-    interface_names_.emplace_back(name_ + "/" + "orientation.z");
-    interface_names_.emplace_back(name_ + "/" + "orientation.w");
-    interface_names_.emplace_back(name_ + "/" + "angular_velocity.x");
-    interface_names_.emplace_back(name_ + "/" + "angular_velocity.y");
-    interface_names_.emplace_back(name_ + "/" + "angular_velocity.z");
-    interface_names_.emplace_back(name_ + "/" + "linear_acceleration.x");
-    interface_names_.emplace_back(name_ + "/" + "linear_acceleration.y");
-    interface_names_.emplace_back(name_ + "/" + "linear_acceleration.z");
-
-    // Set default values to NaN
-    orientation_.fill(std::numeric_limits<double>::quiet_NaN());
-    angular_velocity_.fill(std::numeric_limits<double>::quiet_NaN());
-    linear_acceleration_.fill(std::numeric_limits<double>::quiet_NaN());
   }
-
-  virtual ~IMUSensor() = default;
-
   /// Return orientation.
   /**
    * Return orientation reported by an IMU
    *
-   * \return array of size 4 with orientation quaternion (x,y,z,w)
+   * \return Array of size 4 with orientation quaternion (x,y,z,w).
    */
-  std::array<double, 4> get_orientation()
+  std::array<double, 4> get_orientation() const
   {
-    size_t interface_offset = 0;
-    for (size_t i = 0; i < orientation_.size(); ++i)
+    std::array<double, 4> orientation;
+    for (auto i = 0u; i < orientation.size(); ++i)
     {
-      orientation_[i] = state_interfaces_[interface_offset + i].get().get_value();
+      orientation[i] = state_interfaces_[i].get().get_value();
     }
-    return orientation_;
+    return orientation;
   }
 
   /// Return angular velocity.
   /**
    * Return angular velocity reported by an IMU
    *
-   * \return array of size 3 with angular velocity values.
+   * \return array of size 3 with angular velocity values (x, y, z).
    */
-  std::array<double, 3> get_angular_velocity()
+  std::array<double, 3> get_angular_velocity() const
   {
-    size_t interface_offset = orientation_.size();
-    for (size_t i = 0; i < angular_velocity_.size(); ++i)
+    std::array<double, 3> angular_velocity;
+    const std::size_t interface_offset{4};
+    for (auto i = 0u; i < angular_velocity.size(); ++i)
     {
-      angular_velocity_[i] = state_interfaces_[interface_offset + i].get().get_value();
+      angular_velocity[i] = state_interfaces_[interface_offset + i].get().get_value();
     }
-    return angular_velocity_;
+    return angular_velocity;
   }
 
   /// Return linear acceleration.
   /**
    * Return linear acceleration reported by an IMU
    *
-   * \return array of size 3 with linear acceleration values.
+   * \return array of size 3 with linear acceleration values (x, y, z).
    */
-  std::array<double, 3> get_linear_acceleration()
+  std::array<double, 3> get_linear_acceleration() const
   {
-    size_t interface_offset = orientation_.size() + angular_velocity_.size();
-    for (size_t i = 0; i < linear_acceleration_.size(); ++i)
+    std::array<double, 3> linear_acceleration;
+    const std::size_t interface_offset{7};
+    for (auto i = 0u; i < linear_acceleration.size(); ++i)
     {
-      linear_acceleration_[i] = state_interfaces_[interface_offset + i].get().get_value();
+      linear_acceleration[i] = state_interfaces_[interface_offset + i].get().get_value();
     }
-    return linear_acceleration_;
+    return linear_acceleration;
   }
 
   /// Return Imu message with orientation, angular velocity and linear acceleration
   /**
    * Constructs and return a IMU message from the current values.
    * \return imu message from values;
    */
-  bool get_values_as_message(sensor_msgs::msg::Imu & message)
+  bool get_values_as_message(sensor_msgs::msg::Imu & message) const
   {
-    // call get_orientation() and get_angular_velocity()  get_linear_acceleration() to
-    // update with the latest values
-    get_orientation();
-    get_angular_velocity();
-    get_linear_acceleration();
+    const auto [orientation_x, orientation_y, orientation_z, orientation_w] = get_orientation();
+    const auto [angular_velocity_x, angular_velocity_y, angular_velocity_z] =
+      get_angular_velocity();
+    const auto [linear_acceleration_x, linear_acceleration_y, linear_acceleration_z] =
+      get_linear_acceleration();
 
-    // update the message values, covariances unknown
-    message.orientation.x = orientation_[0];
-    message.orientation.y = orientation_[1];
-    message.orientation.z = orientation_[2];
-    message.orientation.w = orientation_[3];
+    message.orientation.x = orientation_x;
+    message.orientation.y = orientation_y;
+    message.orientation.z = orientation_z;
+    message.orientation.w = orientation_w;
 
-    message.angular_velocity.x = angular_velocity_[0];
-    message.angular_velocity.y = angular_velocity_[1];
-    message.angular_velocity.z = angular_velocity_[2];
+    message.angular_velocity.x = angular_velocity_x;
+    message.angular_velocity.y = angular_velocity_y;
+    message.angular_velocity.z = angular_velocity_z;
 
-    message.linear_acceleration.x = linear_acceleration_[0];
-    message.linear_acceleration.y = linear_acceleration_[1];
-    message.linear_acceleration.z = linear_acceleration_[2];
+    message.linear_acceleration.x = linear_acceleration_x;
+    message.linear_acceleration.y = linear_acceleration_y;
+    message.linear_acceleration.z = linear_acceleration_z;
 
     return true;
   }
-
-protected:
-  // Order is: orientation X,Y,Z,W angular velocity X,Y,Z and linear acceleration X,Y,Z
-  std::array<double, 4> orientation_;
-  std::array<double, 3> angular_velocity_;
-  std::array<double, 3> linear_acceleration_;
 };
 
 }  // namespace semantic_components