Add GPS semantic component

controller_interface/CMakeLists.txt

@@ -86,6 +86,12 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_pose_sensor
     geometry_msgs
   )
+
+  ament_add_gmock(test_gps_sensor test/test_gps_sensor.cpp)
+  target_link_libraries(test_gps_sensor
+    controller_interface
+    hardware_interface::hardware_interface
+  )
 endif()
 
 install(

controller_interface/include/semantic_components/gps_sensor.hpp

@@ -0,0 +1,125 @@
+// Copyright 2025 ros2_control development team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_
+#define SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_
+
+#include <array>
+#include <string>
+#include <vector>
+
+#include "semantic_components/semantic_component_interface.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+
+namespace semantic_components
+{
+
+template <bool use_covariance>
+class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
+{
+public:
+  explicit GPSSensor(const std::string & name)
+  : SemanticComponentInterface(
+      name, {{name + "/" + "status"},
+             {name + "/" + "service"},
+             {name + "/" + "latitude"},
+             {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 Status
+   */
+  int8_t get_status() const { return static_cast<int8_t>(state_interfaces_[0].get().get_value()); }
+
+  /**
+   * Return service used by GPS e.g. fix/no_fix
+   *
+   * \return Service
+   */
+  uint16_t get_service() const
+  {
+    return static_cast<uint16_t>(state_interfaces_[1].get().get_value());
+  }
+
+  /**
+   * Return latitude reported by a GPS
+   *
+   * \return Latitude.
+   */
+  double get_latitude() const { return state_interfaces_[2].get().get_value(); }
+
+  /**
+   * Return longitude reported by a GPS
+   *
+   * \return Longitude.
+   */
+  double get_longitude() const { return state_interfaces_[3].get().get_value(); }
+
+  /**
+   * Return altitude reported by a GPS
+   *
+   * \return Altitude.
+   */
+  double get_altitude() const { return state_interfaces_[4].get().get_value(); }
+
+  /**
+   * Return covariance reported by a GPS.
+   *
+   * \return Covariance array.
+   */
+  template <typename U = void, typename = std::enable_if_t<use_covariance, U>>
+  const std::array<double, 9> & get_covariance()
+  {
+    covariance_[0] = state_interfaces_[5].get().get_value();
+    covariance_[4] = state_interfaces_[6].get().get_value();
+    covariance_[8] = state_interfaces_[7].get().get_value();
+    return covariance_;
+  }
+
+  /**
+   * Fills a NavSatFix message from the current values.
+   */
+  bool get_values_as_message(sensor_msgs::msg::NavSatFix & message)
+  {
+    message.status.status = get_status();
+    message.status.service = get_service();
+    message.latitude = get_latitude();
+    message.longitude = get_longitude();
+    message.altitude = get_altitude();
+
+    if constexpr (use_covariance)
+    {
+      message.position_covariance = get_covariance();
+    }
+
+    return true;
+  }
+
+private:
+  std::array<double, 9> covariance_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
+};
+
+}  // namespace semantic_components
+
+#endif  // SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_

controller_interface/test/test_gps_sensor.cpp

@@ -0,0 +1,239 @@
+// Copyright 2021 ros2_control development team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <algorithm>
+#include <array>
+#include <iterator>
+#include <string>
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
+#include "semantic_components/gps_sensor.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+
+struct GPSSensorTest : public testing::Test
+{
+  GPSSensorTest()
+  {
+    std::transform(
+      gps_interface_names.cbegin(), gps_interface_names.cend(),
+      std::back_inserter(full_interface_names),
+      [this](const auto & interface_name) { return gps_sensor_name + '/' + interface_name; });
+    state_interface.emplace_back(gps_state);
+    state_interface.emplace_back(gps_service);
+    state_interface.emplace_back(latitude);
+    state_interface.emplace_back(longitude);
+    state_interface.emplace_back(altitude);
+  }
+
+  const std::string gps_sensor_name{"gps_sensor"};
+  const std::array<std::string, 5> gps_interface_names{
+    {"status", "service", "latitude", "longitude", "altitude"}};
+  std::array<double, 5> gps_states{};
+  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{
+    gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)};
+  hardware_interface::StateInterface gps_service{
+    gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)};
+  hardware_interface::StateInterface latitude{
+    gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)};
+  hardware_interface::StateInterface longitude{
+    gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)};
+  hardware_interface::StateInterface altitude{
+    gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)};
+  std::vector<hardware_interface::LoanedStateInterface> state_interface;
+};
+
+TEST_F(
+  GPSSensorTest,
+  interfaces_name_should_contain_status_latitude_longitude_altitude_and_sensor_name_prefix)
+{
+  const auto senors_interfaces_name = sut.get_state_interface_names();
+  EXPECT_THAT(senors_interfaces_name, testing::ElementsAreArray(full_interface_names));
+}
+
+TEST_F(
+  GPSSensorTest,
+  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());
+  EXPECT_EQ(gps_states.at(1), sut.get_service());
+  EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_latitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(3), sut.get_longitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(4), sut.get_altitude());
+
+  gps_states.at(0) = 1.0;
+  gps_states.at(1) = 3.0;
+  gps_states.at(2) = 2.0;
+  gps_states.at(3) = 3.0;
+  gps_states.at(4) = 4.0;
+
+  EXPECT_EQ(gps_states.at(0), sut.get_status());
+  EXPECT_EQ(gps_states.at(1), sut.get_service());
+  EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_latitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(3), sut.get_longitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(4), sut.get_altitude());
+}
+
+TEST_F(GPSSensorTest, should_fill_gps_nav_sat_fix_msg_with_value_from_state_interface)
+{
+  EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
+
+  sensor_msgs::msg::NavSatFix message;
+  EXPECT_TRUE(sut.get_values_as_message(message));
+  EXPECT_EQ(gps_states.at(0), message.status.status);
+  EXPECT_EQ(gps_states.at(1), message.status.service);
+  EXPECT_DOUBLE_EQ(gps_states.at(2), message.latitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(3), message.longitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(4), message.altitude);
+
+  gps_states.at(0) = 1.0;
+  gps_states.at(1) = 3.0;
+  gps_states.at(2) = 2.0;
+  gps_states.at(3) = 3.0;
+  gps_states.at(4) = 4.0;
+
+  EXPECT_TRUE(sut.get_values_as_message(message));
+  EXPECT_EQ(gps_states.at(0), message.status.status);
+  EXPECT_EQ(gps_states.at(1), message.status.service);
+  EXPECT_DOUBLE_EQ(gps_states.at(2), message.latitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(3), message.longitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(4), message.altitude);
+}
+
+struct GPSSensorWithCovarianceTest : public testing::Test
+{
+  GPSSensorWithCovarianceTest()
+  {
+    std::transform(
+      gps_interface_names.cbegin(), gps_interface_names.cend(),
+      std::back_inserter(full_interface_names),
+      [this](const auto & interface_name) { return gps_sensor_name + '/' + interface_name; });
+    state_interface.emplace_back(gps_state);
+    state_interface.emplace_back(gps_service);
+    state_interface.emplace_back(latitude);
+    state_interface.emplace_back(longitude);
+    state_interface.emplace_back(altitude);
+    state_interface.emplace_back(latitude_covariance);
+    state_interface.emplace_back(longitude_covariance);
+    state_interface.emplace_back(altitude_covariance);
+  }
+
+  const std::string gps_sensor_name{"gps_sensor"};
+  const std::array<std::string, 8> gps_interface_names{
+    {"status", "service", "latitude", "longitude", "altitude", "latitude_covariance",
+     "longitude_covariance", "altitude_covariance"}};
+  std::array<double, 8> gps_states{};
+  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{
+    gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)};
+  hardware_interface::StateInterface gps_service{
+    gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)};
+  hardware_interface::StateInterface latitude{
+    gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)};
+  hardware_interface::StateInterface longitude{
+    gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)};
+  hardware_interface::StateInterface altitude{
+    gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)};
+  hardware_interface::StateInterface latitude_covariance{
+    gps_sensor_name, gps_interface_names.at(5), &gps_states.at(5)};
+  hardware_interface::StateInterface longitude_covariance{
+    gps_sensor_name, gps_interface_names.at(6), &gps_states.at(6)};
+  hardware_interface::StateInterface altitude_covariance{
+    gps_sensor_name, gps_interface_names.at(7), &gps_states.at(7)};
+  std::vector<hardware_interface::LoanedStateInterface> state_interface;
+};
+
+TEST_F(
+  GPSSensorWithCovarianceTest,
+  interfaces_name_should_contain_status_latitude_longitude_altitude_and_sensor_name_prefix)
+{
+  const auto senors_interfaces_name = sut.get_state_interface_names();
+
+  EXPECT_EQ(senors_interfaces_name.size(), full_interface_names.size());
+  EXPECT_THAT(senors_interfaces_name, testing::ElementsAreArray(full_interface_names));
+}
+
+TEST_F(
+  GPSSensorWithCovarianceTest,
+  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());
+  EXPECT_EQ(gps_states.at(1), sut.get_service());
+  EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_latitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(3), sut.get_longitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(4), sut.get_altitude());
+  EXPECT_THAT(
+    sut.get_covariance(), testing::ElementsAreArray({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
+
+  gps_states.at(0) = 1.0;
+  gps_states.at(1) = 3.0;
+  gps_states.at(2) = 2.0;
+  gps_states.at(3) = 3.0;
+  gps_states.at(4) = 4.0;
+  gps_states.at(5) = 0.5;
+  gps_states.at(6) = 0.2;
+  gps_states.at(7) = 0.7;
+
+  EXPECT_EQ(gps_states.at(0), sut.get_status());
+  EXPECT_EQ(gps_states.at(1), sut.get_service());
+  EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_latitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(3), sut.get_longitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(4), sut.get_altitude());
+  EXPECT_THAT(
+    sut.get_covariance(), testing::ElementsAreArray({0.5, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.7}));
+}
+
+TEST_F(GPSSensorWithCovarianceTest, should_fill_gps_nav_sat_fix_msg_with_value_from_state_interface)
+{
+  EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
+  sensor_msgs::msg::NavSatFix message;
+  EXPECT_TRUE(sut.get_values_as_message(message));
+  EXPECT_EQ(gps_states.at(0), message.status.status);
+  EXPECT_EQ(gps_states.at(1), message.status.service);
+  EXPECT_DOUBLE_EQ(gps_states.at(2), message.latitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(3), message.longitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(4), message.altitude);
+  EXPECT_THAT(
+    message.position_covariance,
+    testing::ElementsAreArray({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
+
+  gps_states.at(0) = 1.0;
+  gps_states.at(1) = 2.0;
+  gps_states.at(2) = 2.0;
+  gps_states.at(3) = 3.0;
+  gps_states.at(4) = 4.0;
+  gps_states.at(5) = 0.5;
+  gps_states.at(6) = 0.2;
+  gps_states.at(7) = 0.7;
+
+  EXPECT_TRUE(sut.get_values_as_message(message));
+  EXPECT_EQ(gps_states.at(0), message.status.status);
+  EXPECT_EQ(gps_states.at(1), message.status.service);
+  EXPECT_DOUBLE_EQ(gps_states.at(2), message.latitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(3), message.longitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(4), message.altitude);
+  EXPECT_THAT(
+    message.position_covariance,
+    testing::ElementsAreArray({0.5, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.7}));
+}