Merge branch 'master' into integrate/pal_statistics

Author: saikishor

controller_interface/CMakeLists.txt

@@ -45,11 +45,12 @@ if(BUILD_TESTING)
   )
 
   ament_add_gmock(test_controller_with_options test/test_controller_with_options.cpp)
-  install(FILES test/test_controller_node_options.yaml
-      DESTINATION test)
   target_link_libraries(test_controller_with_options
     controller_interface
   )
+  target_compile_definitions(
+    test_controller_with_options
+    PRIVATE PARAMETERS_FILE_PATH="${CMAKE_CURRENT_LIST_DIR}/test/")
 
   ament_add_gmock(test_chainable_controller_interface test/test_chainable_controller_interface.cpp)
   target_link_libraries(test_chainable_controller_interface
@@ -86,6 +87,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,136 @@
+// 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 "semantic_components/semantic_component_interface.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+
+namespace semantic_components
+{
+
+enum class GPSSensorOption
+{
+  WithCovariance,
+  WithoutCovariance
+};
+
+template <GPSSensorOption sensor_option>
+class GPSSensor : public SemanticComponentInterface<sensor_msgs::msg::NavSatFix>
+{
+public:
+  static_assert(
+    sensor_option == GPSSensorOption::WithCovariance ||
+      sensor_option == GPSSensorOption::WithoutCovariance,
+    "Invalid GPSSensorOption");
+  explicit GPSSensor(const std::string & name)
+  : SemanticComponentInterface(
+      name, {{name + "/" + "status"},
+             {name + "/" + "service"},
+             {name + "/" + "latitude"},
+             {name + "/" + "longitude"},
+             {name + "/" + "altitude"}})
+  {
+    if constexpr (sensor_option == GPSSensorOption::WithCovariance)
+    {
+      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<sensor_option == GPSSensorOption::WithCovariance, 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 (sensor_option == GPSSensorOption::WithCovariance)
+    {
+      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_controller_with_options.cpp

@@ -16,7 +16,6 @@
 
 #include <gtest/gtest.h>
 #include <string>
-#include "ament_index_cpp/get_package_prefix.hpp"
 
 class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions
 {
@@ -95,8 +94,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") +
-                                       "/test/test_controller_node_options.yaml";
+  const std::string params_file_path =
+    std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_node_options.yaml");
   std::cerr << params_file_path << std::endl;
   auto controller_node_options = controller.define_custom_node_options();
   controller_node_options.arguments({"--ros-args", "--params-file", params_file_path});
@@ -129,8 +128,8 @@ TEST(
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") +
-                                       "/test/test_controller_node_options.yaml";
+  const std::string params_file_path =
+    std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_node_options.yaml");
   std::cerr << params_file_path << std::endl;
   auto controller_node_options = controller.define_custom_node_options();
   controller_node_options.arguments(