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,118 @@
+// 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 <string>
+#include <vector>
+
+#include "semantic_components/semantic_component_interface.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+
+namespace semantic_components
+{
+class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
+{
+public:
+  explicit GPSSensor(const std::string & name)
+  : SemanticComponentInterface(
+      name, {{name + "/" + "status"},
+             {name + "/" + "latitude"},
+             {name + "/" + "longitude"},
+             {name + "/" + "altitude"}})
+  {
+  }
+
+  /**
+   * 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()); }
+
+  /**
+   * Return latitude reported by a GPS
+   *
+   * \return Latitude.
+   */
+  double get_latitude() { 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(); }
+
+  /**
+   * 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");
+  }
+
+  /**
+   * Return covariance reported by a GPS.
+   *
+   * \return Covariance array.
+   */
+  std::array<double, 9> get_covariance()
+  {
+    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()}});
+  }
+
+  /**
+   * Fills a NavSatFix message with the current values,
+   * including the diagonal elements of the position covariance.
+   * \return true
+   */
+  bool get_values_as_message(sensor_msgs::msg::NavSatFix & message) override
+  {
+    GPSSensor::get_values_as_message(message);
+    message.position_covariance = get_covariance();
+    return true;
+  }
+};
+
+}  // namespace semantic_components
+
+#endif  // SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_

controller_interface/test/test_gps_sensor.cpp

@@ -0,0 +1,219 @@
+// 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(latitude);
+    state_interface.emplace_back(longitude);
+    state_interface.emplace_back(altitude);
+  }
+
+  const std::string gps_sensor_name{"gps_sensor"};
+  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};
+  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 latitude{
+    gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)};
+  hardware_interface::StateInterface longitude{
+    gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)};
+  hardware_interface::StateInterface altitude{
+    gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)};
+  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_corepending_values_in_state_interface)
+{
+  EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
+  EXPECT_EQ(gps_states.at(0), sut.get_status());
+  EXPECT_DOUBLE_EQ(gps_states.at(1), sut.get_latitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_longitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(3), sut.get_altitude());
+
+  gps_states.at(0) = 1.0;
+  gps_states.at(1) = 2.0;
+  gps_states.at(2) = 3.0;
+  gps_states.at(3) = 4.0;
+
+  EXPECT_EQ(gps_states.at(0), sut.get_status());
+  EXPECT_DOUBLE_EQ(gps_states.at(1), sut.get_latitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_longitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(3), 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_DOUBLE_EQ(gps_states.at(1), message.latitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(2), message.longitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(3), message.altitude);
+
+  gps_states.at(0) = 1.0;
+  gps_states.at(1) = 2.0;
+  gps_states.at(2) = 3.0;
+  gps_states.at(3) = 4.0;
+
+  EXPECT_TRUE(sut.get_values_as_message(message));
+  EXPECT_EQ(gps_states.at(0), message.status.status);
+  EXPECT_DOUBLE_EQ(gps_states.at(1), message.latitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(2), message.longitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(3), 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(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, 7> gps_interface_names{
+    {"status", "latitude", "longitude", "altitude", "latitude_covariance", "longitude_covariance",
+     "altitude_covariance"}};
+  std::array<double, 7> gps_states{};
+  semantic_components::GPSSensorWithCovariance 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 latitude{
+    gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)};
+  hardware_interface::StateInterface longitude{
+    gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)};
+  hardware_interface::StateInterface altitude{
+    gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)};
+  hardware_interface::StateInterface latitude_covariance{
+    gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)};
+  hardware_interface::StateInterface longitude_covariance{
+    gps_sensor_name, gps_interface_names.at(5), &gps_states.at(5)};
+  hardware_interface::StateInterface altitude_covariance{
+    gps_sensor_name, gps_interface_names.at(6), &gps_states.at(6)};
+  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_corepending_values_in_state_interface)
+{
+  EXPECT_TRUE(sut.assign_loaned_state_interfaces(state_interface));
+  EXPECT_EQ(gps_states.at(0), sut.get_status());
+  EXPECT_DOUBLE_EQ(gps_states.at(1), sut.get_latitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_longitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(3), 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) = 2.0;
+  gps_states.at(2) = 3.0;
+  gps_states.at(3) = 4.0;
+  gps_states.at(4) = 0.5;
+  gps_states.at(5) = 0.2;
+  gps_states.at(6) = 0.7;
+
+  EXPECT_EQ(gps_states.at(0), sut.get_status());
+  EXPECT_DOUBLE_EQ(gps_states.at(1), sut.get_latitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(2), sut.get_longitude());
+  EXPECT_DOUBLE_EQ(gps_states.at(3), 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_DOUBLE_EQ(gps_states.at(1), message.latitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(2), message.longitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(3), 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) = 3.0;
+  gps_states.at(3) = 4.0;
+  gps_states.at(4) = 0.5;
+  gps_states.at(5) = 0.2;
+  gps_states.at(6) = 0.7;
+
+  EXPECT_TRUE(sut.get_values_as_message(message));
+  EXPECT_EQ(gps_states.at(0), message.status.status);
+  EXPECT_DOUBLE_EQ(gps_states.at(1), message.latitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(2), message.longitude);
+  EXPECT_DOUBLE_EQ(gps_states.at(3), 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}));
+}