Merge GPSSensor classes in to one

ros-controls · Jan 14, 2025 · 16da7e9 · 16da7e9
1 parent 1a02b2e
commit 16da7e9
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 43 deletions.
diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp
@@ -23,6 +23,8 @@
 
 namespace semantic_components
 {
+
+template <bool use_covariance>
 class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
 {
 public:
@@ -33,82 +35,71 @@ class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
              {name + "/" + "longitude"},
              {name + "/" + "altitude"}})
   {
+    if constexpr (use_covariance)
+    {
+      interface_names_.emplace_back(name + "/" + "latitude_covariance");
+      interface_names_.emplace_back(name + "/" + "longitude_covariance");
+      interface_names_.emplace_back(name + "/" + "altitude_covariance");
+    }
   }
 
   /**
    * Return GPS's status e.g. fix/no_fix
    *
    * \return Latitude.
    */
-  int8_t get_status() { return static_cast<int8_t>(state_interfaces_[0].get().get_value()); }
+  int8_t get_status() const { return static_cast<int8_t>(state_interfaces_[0].get().get_value()); }
 
   /**
    * Return latitude reported by a GPS
    *
    * \return Latitude.
    */
-  double get_latitude() { return state_interfaces_[1].get().get_value(); }
+  double get_latitude() const { return state_interfaces_[1].get().get_value(); }
 
   /**
    * Return longitude reported by a GPS
    *
    * \return Longitude.
    */
-  double get_longitude() { return state_interfaces_[2].get().get_value(); }
+  double get_longitude() const { return state_interfaces_[2].get().get_value(); }
 
   /**
    * Return altitude reported by a GPS
    *
    * \return Altitude.
    */
-  double get_altitude() { return state_interfaces_[3].get().get_value(); }
-
-  /**
-   * Fills a NavSatFix message from the current values.
-   */
-  virtual bool get_values_as_message(sensor_msgs::msg::NavSatFix & message)
-  {
-    message.status.status = get_status();
-    message.latitude = get_latitude();
-    message.longitude = get_longitude();
-    message.altitude = get_altitude();
-
-    return true;
-  }
-};
-
-class GPSSensorWithCovariance : public GPSSensor
-{
-public:
-  explicit GPSSensorWithCovariance(const std::string & name) : GPSSensor(name)
-  {
-    interface_names_.emplace_back(name + "/" + "latitude_covariance");
-    interface_names_.emplace_back(name + "/" + "longitude_covariance");
-    interface_names_.emplace_back(name + "/" + "altitude_covariance");
-  }
+  double get_altitude() const { return state_interfaces_[3].get().get_value(); }
 
   /**
    * Return covariance reported by a GPS.
    *
    * \return Covariance array.
    */
-  std::array<double, 9> get_covariance()
+  template <typename U = void, typename = std::enable_if_t<use_covariance, U>>
+  std::array<double, 9> get_covariance() const
   {
-    return std::array<double, 9>(
-      {{state_interfaces_[4].get().get_value(), 0.0, 0.0, 0.0,
-        state_interfaces_[5].get().get_value(), 0.0, 0.0, 0.0,
-        state_interfaces_[6].get().get_value()}});
+    return {
+      {state_interfaces_[4].get().get_value(), 0.0, 0.0, 0.0,
+       state_interfaces_[5].get().get_value(), 0.0, 0.0, 0.0,
+       state_interfaces_[6].get().get_value()}};
   }
 
   /**
-   * Fills a NavSatFix message with the current values,
-   * including the diagonal elements of the position covariance.
-   * \return true
+   * Fills a NavSatFix message from the current values.
    */
-  bool get_values_as_message(sensor_msgs::msg::NavSatFix & message) override
+  bool get_values_as_message(sensor_msgs::msg::NavSatFix & message)
   {
-    GPSSensor::get_values_as_message(message);
-    message.position_covariance = get_covariance();
+    message.status.status = get_status();
+    message.latitude = get_latitude();
+    message.longitude = get_longitude();
+    message.altitude = get_altitude();
+
+    if constexpr (use_covariance)
+    {
+      message.position_covariance = get_covariance();
+    }
+
     return true;
   }
 };

diff --git a/controller_interface/test/test_gps_sensor.cpp b/controller_interface/test/test_gps_sensor.cpp
@@ -41,7 +41,8 @@ struct GPSSensorTest : public testing::Test
   const std::array<std::string, 4> gps_interface_names{
     {"status", "latitude", "longitude", "altitude"}};
   std::array<double, 4> gps_states{};
-  semantic_components::GPSSensor sut{gps_sensor_name};
+  static constexpr bool use_covariance{false};
+  semantic_components::GPSSensor<use_covariance> sut{gps_sensor_name};
   std::vector<std::string> full_interface_names;
 
   hardware_interface::StateInterface gps_state{
@@ -65,7 +66,7 @@ TEST_F(
 
 TEST_F(
   GPSSensorTest,
-  status_latitude_longitude_altitude_should_be_equal_to_corepending_values_in_state_interface)
+  status_latitude_longitude_altitude_should_be_equal_to_corresponding_values_in_state_interface)
 {
   EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
   EXPECT_EQ(gps_states.at(0), sut.get_status());
@@ -129,7 +130,8 @@ struct GPSSensorWithCovarianceTest : public testing::Test
     {"status", "latitude", "longitude", "altitude", "latitude_covariance", "longitude_covariance",
      "altitude_covariance"}};
   std::array<double, 7> gps_states{};
-  semantic_components::GPSSensorWithCovariance sut{gps_sensor_name};
+  static constexpr bool use_covariance{true};
+  semantic_components::GPSSensor<use_covariance> sut{gps_sensor_name};
   std::vector<std::string> full_interface_names;
 
   hardware_interface::StateInterface gps_state{
@@ -161,7 +163,7 @@ TEST_F(
 
 TEST_F(
   GPSSensorWithCovarianceTest,
-  status_latitude_longitude_altitude_should_be_equal_to_corepending_values_in_state_interface)
+  status_latitude_longitude_altitude_should_be_equal_to_corresponding_values_in_state_interface)
 {
   EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
   EXPECT_EQ(gps_states.at(0), sut.get_status());