[Handle] Use get_optional instead of get_value<double>

controller_interface/include/semantic_components/force_torque_sensor.hpp

@@ -15,6 +15,7 @@
 #ifndef SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
 #define SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
 
+#include <algorithm>
 #include <limits>
 #include <string>
 #include <vector>
@@ -25,7 +26,6 @@
 
 namespace semantic_components
 {
-constexpr std::size_t FORCES_SIZE = 3;
 class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::Wrench>
 {
 public:
@@ -42,6 +42,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
        {name + "/" + "torque.z"}}),
     existing_axes_({{true, true, true, true, true, true}})
   {
+    data_.fill(std::numeric_limits<double>::quiet_NaN());
   }
 
   /// Constructor for 6D FTS with custom interface names.
@@ -59,6 +60,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
     const std::string & interface_torque_y, const std::string & interface_torque_z)
   : SemanticComponentInterface("", 6)
   {
+    data_.fill(std::numeric_limits<double>::quiet_NaN());
     auto check_and_add_interface =
       [this](const std::string & interface_name, const std::size_t index)
     {
@@ -89,17 +91,9 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
    */
   std::array<double, 3> get_forces() const
   {
+    update_data_from_interfaces();
     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<double>().value();
-        ++interface_counter;
-      }
-    }
+    std::copy(data_.begin(), data_.begin() + 3, forces.begin());
     return forces;
   }
 
@@ -111,22 +105,9 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
    */
   std::array<double, 3> get_torques() const
   {
+    update_data_from_interfaces();
     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<std::size_t>(
-      std::count(existing_axes_.begin(), existing_axes_.begin() + FORCES_SIZE, true));
-
-    for (auto i = 0u; i < torques.size(); ++i)
-    {
-      if (existing_axes_[i + FORCES_SIZE])
-      {
-        torques[i] = state_interfaces_[torque_interface_counter].get().get_value<double>().value();
-        ++torque_interface_counter;
-      }
-    }
+    std::copy(data_.begin() + 3, data_.end(), torques.begin());
     return torques;
   }
 
@@ -140,20 +121,43 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
    */
   bool get_values_as_message(geometry_msgs::msg::Wrench & message) const
   {
-    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;
+    update_data_from_interfaces();
+    message.force.x = data_[0];
+    message.force.y = data_[1];
+    message.force.z = data_[2];
+    message.torque.x = data_[3];
+    message.torque.y = data_[4];
+    message.torque.z = data_[5];
 
     return true;
   }
 
 protected:
+  /**
+   * @brief Update the data from the state interfaces.
+   * @note The method is thread-safe and non-blocking.
+   * @note This method might return stale data if the data is not updated. This is to ensure that
+   * the data from the sensor is not discontinuous.
+   */
+  void update_data_from_interfaces() const
+  {
+    std::size_t interface_counter{0};
+    for (auto i = 0u; i < data_.size(); ++i)
+    {
+      if (existing_axes_[i])
+      {
+        const auto data = state_interfaces_[interface_counter].get().get_optional();
+        if (data.has_value())
+        {
+          data_[i] = data.value();
+        }
+        ++interface_counter;
+      }
+    }
+  }
+
+  /// Array to store the data of the FT sensors
+  mutable std::array<double, 6> data_;
   /// Vector with existing axes for sensors with less then 6D axes.
   std::array<bool, 6> existing_axes_;
 };

controller_interface/include/semantic_components/gps_sensor.hpp

@@ -16,6 +16,7 @@
 #define SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_
 
 #include <array>
+#include <limits>
 #include <string>
 
 #include "semantic_components/semantic_component_interface.hpp"
@@ -61,7 +62,12 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
    */
   int8_t get_status() const
   {
-    return static_cast<int8_t>(state_interfaces_[0].get().template get_value<double>().value());
+    const auto data = state_interfaces_[0].get().get_optional();
+    if (data.has_value())
+    {
+      return static_cast<int8_t>(data.value());
+    }
+    return std::numeric_limits<int8_t>::max();
   }
 
   /**
@@ -71,7 +77,12 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
    */
   uint16_t get_service() const
   {
-    return static_cast<uint16_t>(state_interfaces_[1].get().template get_value<double>().value());
+    const auto data = state_interfaces_[1].get().get_optional();
+    if (data.has_value())
+    {
+      return static_cast<uint16_t>(data.value());
+    }
+    return std::numeric_limits<uint16_t>::max();
   }
 
   /**
@@ -81,7 +92,12 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
    */
   double get_latitude() const
   {
-    return state_interfaces_[2].get().template get_value<double>().value();
+    const auto data = state_interfaces_[2].get().get_optional();
+    if (data.has_value())
+    {
+      return data.value();
+    }
+    return std::numeric_limits<double>::quiet_NaN();
   }
 
   /**
@@ -91,7 +107,12 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
    */
   double get_longitude() const
   {
-    return state_interfaces_[3].get().template get_value<double>().value();
+    const auto data = state_interfaces_[3].get().get_optional();
+    if (data.has_value())
+    {
+      return data.value();
+    }
+    return std::numeric_limits<double>::quiet_NaN();
   }
 
   /**
@@ -101,7 +122,12 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
    */
   double get_altitude() const
   {
-    return state_interfaces_[4].get().template get_value<double>().value();
+    const auto data = state_interfaces_[4].get().get_optional();
+    if (data.has_value())
+    {
+      return data.value();
+    }
+    return std::numeric_limits<double>::quiet_NaN();
   }
 
   /**
@@ -114,9 +140,21 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
     typename = std::enable_if_t<sensor_option == GPSSensorOption::WithCovariance, U>>
   const std::array<double, 9> & get_covariance()
   {
-    covariance_[0] = state_interfaces_[5].get().template get_value<double>().value();
-    covariance_[4] = state_interfaces_[6].get().template get_value<double>().value();
-    covariance_[8] = state_interfaces_[7].get().template get_value<double>().value();
+    const auto data_1 = state_interfaces_[5].get().get_optional();
+    const auto data_2 = state_interfaces_[6].get().get_optional();
+    const auto data_3 = state_interfaces_[7].get().get_optional();
+    if (data_1.has_value())
+    {
+      covariance_[0] = data_1.value();
+    }
+    if (data_2.has_value())
+    {
+      covariance_[4] = data_2.value();
+    }
+    if (data_3.has_value())
+    {
+      covariance_[8] = data_3.value();
+    }
     return covariance_;
   }
 

controller_interface/include/semantic_components/imu_sensor.hpp

@@ -15,6 +15,7 @@
 #ifndef SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
 #define SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
 
+#include <algorithm>
 #include <limits>
 #include <string>
 #include <vector>
@@ -49,11 +50,9 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
    */
   std::array<double, 4> get_orientation() const
   {
+    update_data_from_interfaces();
     std::array<double, 4> orientation;
-    for (auto i = 0u; i < orientation.size(); ++i)
-    {
-      orientation[i] = state_interfaces_[i].get().get_value<double>().value();
-    }
+    std::copy(data_.begin(), data_.begin() + 4, orientation.begin());
     return orientation;
   }
 
@@ -65,13 +64,9 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
    */
   std::array<double, 3> get_angular_velocity() const
   {
+    update_data_from_interfaces();
     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<double>().value();
-    }
+    std::copy(data_.begin() + 4, data_.begin() + 7, angular_velocity.begin());
     return angular_velocity;
   }
 
@@ -83,13 +78,9 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
    */
   std::array<double, 3> get_linear_acceleration() const
   {
+    update_data_from_interfaces();
     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<double>().value();
-    }
+    std::copy(data_.begin() + 7, data_.end(), linear_acceleration.begin());
     return linear_acceleration;
   }
 
@@ -100,27 +91,44 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
    */
   bool get_values_as_message(sensor_msgs::msg::Imu & message) const
   {
-    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_data_from_interfaces();
+    message.orientation.x = data_[0];
+    message.orientation.y = data_[1];
+    message.orientation.z = data_[2];
+    message.orientation.w = data_[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 = data_[4];
+    message.angular_velocity.y = data_[5];
+    message.angular_velocity.z = data_[6];
 
-    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_x;
-    message.linear_acceleration.y = linear_acceleration_y;
-    message.linear_acceleration.z = linear_acceleration_z;
+    message.linear_acceleration.x = data_[7];
+    message.linear_acceleration.y = data_[8];
+    message.linear_acceleration.z = data_[9];
 
     return true;
   }
+
+private:
+  /**
+   * @brief Update the data array from the state interfaces.
+   * @note This method is thread-safe and non-blocking.
+   * @note This method might return stale data if the data is not updated. This is to ensure that
+   * the data from the sensor is not discontinuous.
+   */
+  void update_data_from_interfaces() const
+  {
+    for (auto i = 0u; i < data_.size(); ++i)
+    {
+      const auto data = state_interfaces_[i].get().get_optional();
+      if (data.has_value())
+      {
+        data_[i] = data.value();
+      }
+    }
+  }
+
+  // Array to store the data of the IMU sensor
+  mutable std::array<double, 10> data_{{0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
 };
 
 }  // namespace semantic_components