From 16da7e91e0fcda44b487dd2e7030d06b30971615 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 14 Jan 2025 09:00:00 +0100 Subject: [PATCH] Merge GPSSensor classes in to one --- .../semantic_components/gps_sensor.hpp | 69 ++++++++----------- controller_interface/test/test_gps_sensor.cpp | 10 +-- 2 files changed, 36 insertions(+), 43 deletions(-) diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp index 8353b2dd30..0709ed0520 100644 --- 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 class GPSSensor : public SemanticComponentInterface { public: @@ -33,6 +35,12 @@ class GPSSensor : public SemanticComponentInterface {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"); + } } /** @@ -40,75 +48,58 @@ class GPSSensor : public SemanticComponentInterface * * \return Latitude. */ - int8_t get_status() { return static_cast(state_interfaces_[0].get().get_value()); } + int8_t get_status() const { return static_cast(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 get_covariance() + template > + std::array get_covariance() const { - return std::array( - {{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 index e6617cd38b..41e00074da 100644 --- 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 gps_interface_names{ {"status", "latitude", "longitude", "altitude"}}; std::array gps_states{}; - semantic_components::GPSSensor sut{gps_sensor_name}; + static constexpr bool use_covariance{false}; + semantic_components::GPSSensor sut{gps_sensor_name}; std::vector 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 gps_states{}; - semantic_components::GPSSensorWithCovariance sut{gps_sensor_name}; + static constexpr bool use_covariance{true}; + semantic_components::GPSSensor sut{gps_sensor_name}; std::vector 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());