From 1a02b2e999af1eac43290917287dd525bf38ef37 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Mon, 13 Jan 2025 09:37:48 +0100 Subject: [PATCH 1/8] Add GPS semantic component. --- controller_interface/CMakeLists.txt | 6 + .../semantic_components/gps_sensor.hpp | 118 ++++++++++ controller_interface/test/test_gps_sensor.cpp | 219 ++++++++++++++++++ 3 files changed, 343 insertions(+) create mode 100644 controller_interface/include/semantic_components/gps_sensor.hpp create mode 100644 controller_interface/test/test_gps_sensor.cpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 60b213cd57..dc810eaeb5 100644 --- a/controller_interface/CMakeLists.txt +++ b/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( diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp new file mode 100644 index 0000000000..8353b2dd30 --- /dev/null +++ b/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 +#include + +#include "semantic_components/semantic_component_interface.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +namespace semantic_components +{ +class GPSSensor : public SemanticComponentInterface +{ +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(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 get_covariance() + { + 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()}}); + } + + /** + * 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_ diff --git a/controller_interface/test/test_gps_sensor.cpp b/controller_interface/test/test_gps_sensor.cpp new file mode 100644 index 0000000000..e6617cd38b --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#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 gps_interface_names{ + {"status", "latitude", "longitude", "altitude"}}; + std::array gps_states{}; + semantic_components::GPSSensor sut{gps_sensor_name}; + std::vector 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 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 gps_interface_names{ + {"status", "latitude", "longitude", "altitude", "latitude_covariance", "longitude_covariance", + "altitude_covariance"}}; + std::array gps_states{}; + semantic_components::GPSSensorWithCovariance sut{gps_sensor_name}; + std::vector 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 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})); +} From 16da7e91e0fcda44b487dd2e7030d06b30971615 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 14 Jan 2025 09:00:00 +0100 Subject: [PATCH 2/8] 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()); From f8d0edc9962c2ecc8c1417631a4e2dd1d96a96f0 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 14 Jan 2025 20:54:33 +0100 Subject: [PATCH 3/8] Add service interface --- .../semantic_components/gps_sensor.hpp | 36 +++-- controller_interface/test/test_gps_sensor.cpp | 132 ++++++++++-------- 2 files changed, 102 insertions(+), 66 deletions(-) diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp index 0709ed0520..fe85d3ab12 100644 --- a/controller_interface/include/semantic_components/gps_sensor.hpp +++ b/controller_interface/include/semantic_components/gps_sensor.hpp @@ -15,6 +15,7 @@ #ifndef SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_ #define SEMANTIC_COMPONENTS__GPS_SENSOR_HPP_ +#include #include #include @@ -31,6 +32,7 @@ class GPSSensor : public SemanticComponentInterface explicit GPSSensor(const std::string & name) : SemanticComponentInterface( name, {{name + "/" + "status"}, + {name + "/" + "service"}, {name + "/" + "latitude"}, {name + "/" + "longitude"}, {name + "/" + "altitude"}}) @@ -40,36 +42,44 @@ class GPSSensor : public SemanticComponentInterface interface_names_.emplace_back(name + "/" + "latitude_covariance"); interface_names_.emplace_back(name + "/" + "longitude_covariance"); interface_names_.emplace_back(name + "/" + "altitude_covariance"); + covariance_.fill(0.0); } } /** * Return GPS's status e.g. fix/no_fix * - * \return Latitude. + * \return Status */ int8_t get_status() const { return static_cast(state_interfaces_[0].get().get_value()); } + /** + * Return service used by GPS e.g. fix/no_fix + * + * \return Service + */ + int8_t get_service() const { return static_cast(state_interfaces_[1].get().get_value()); } + /** * Return latitude reported by a GPS * * \return Latitude. */ - double get_latitude() const { return state_interfaces_[1].get().get_value(); } + 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_[2].get().get_value(); } + 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_[3].get().get_value(); } + double get_altitude() const { return state_interfaces_[4].get().get_value(); } /** * Return covariance reported by a GPS. @@ -77,12 +87,12 @@ class GPSSensor : public SemanticComponentInterface * \return Covariance array. */ template > - std::array get_covariance() const + const std::array & get_covariance() { - 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()}}; + 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_; } /** @@ -91,6 +101,7 @@ class GPSSensor : public SemanticComponentInterface 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(); @@ -102,6 +113,13 @@ class GPSSensor : public SemanticComponentInterface return true; } + +private: + struct Empty + { + }; + using CovarianceArray = std::conditional_t, Empty>; + CovarianceArray covariance_; }; } // namespace semantic_components diff --git a/controller_interface/test/test_gps_sensor.cpp b/controller_interface/test/test_gps_sensor.cpp index 41e00074da..5377e18ad6 100644 --- a/controller_interface/test/test_gps_sensor.cpp +++ b/controller_interface/test/test_gps_sensor.cpp @@ -32,27 +32,30 @@ struct GPSSensorTest : public testing::Test 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 gps_interface_names{ - {"status", "latitude", "longitude", "altitude"}}; - std::array gps_states{}; + const std::array gps_interface_names{ + {"status", "service", "latitude", "longitude", "altitude"}}; + std::array gps_states{}; static constexpr bool use_covariance{false}; semantic_components::GPSSensor sut{gps_sensor_name}; std::vector full_interface_names; hardware_interface::StateInterface gps_state{ gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)}; - hardware_interface::StateInterface latitude{ + hardware_interface::StateInterface gps_service{ gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)}; - hardware_interface::StateInterface longitude{ + hardware_interface::StateInterface latitude{ gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)}; - hardware_interface::StateInterface altitude{ + 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 state_interface; }; @@ -70,19 +73,22 @@ TEST_F( { 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_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) = 2.0; - gps_states.at(2) = 3.0; - gps_states.at(3) = 4.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_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_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) @@ -92,20 +98,23 @@ TEST_F(GPSSensorTest, should_fill_gps_nav_sat_fix_msg_with_value_from_state_inte 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_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) = 2.0; - gps_states.at(2) = 3.0; - gps_states.at(3) = 4.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_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_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 @@ -117,6 +126,7 @@ struct GPSSensorWithCovarianceTest : public testing::Test 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); @@ -126,28 +136,30 @@ struct GPSSensorWithCovarianceTest : public testing::Test } const std::string gps_sensor_name{"gps_sensor"}; - const std::array gps_interface_names{ - {"status", "latitude", "longitude", "altitude", "latitude_covariance", "longitude_covariance", - "altitude_covariance"}}; - std::array gps_states{}; + const std::array gps_interface_names{ + {"status", "service", "latitude", "longitude", "altitude", "latitude_covariance", + "longitude_covariance", "altitude_covariance"}}; + std::array gps_states{}; static constexpr bool use_covariance{true}; semantic_components::GPSSensor sut{gps_sensor_name}; std::vector full_interface_names; hardware_interface::StateInterface gps_state{ gps_sensor_name, gps_interface_names.at(0), &gps_states.at(0)}; - hardware_interface::StateInterface latitude{ + hardware_interface::StateInterface gps_service{ gps_sensor_name, gps_interface_names.at(1), &gps_states.at(1)}; - hardware_interface::StateInterface longitude{ + hardware_interface::StateInterface latitude{ gps_sensor_name, gps_interface_names.at(2), &gps_states.at(2)}; - hardware_interface::StateInterface altitude{ + hardware_interface::StateInterface longitude{ gps_sensor_name, gps_interface_names.at(3), &gps_states.at(3)}; - hardware_interface::StateInterface latitude_covariance{ + hardware_interface::StateInterface altitude{ gps_sensor_name, gps_interface_names.at(4), &gps_states.at(4)}; - hardware_interface::StateInterface longitude_covariance{ + hardware_interface::StateInterface latitude_covariance{ gps_sensor_name, gps_interface_names.at(5), &gps_states.at(5)}; - hardware_interface::StateInterface altitude_covariance{ + 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 state_interface; }; @@ -167,24 +179,27 @@ TEST_F( { 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_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) = 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; + 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_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_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})); } @@ -195,26 +210,29 @@ TEST_F(GPSSensorWithCovarianceTest, should_fill_gps_nav_sat_fix_msg_with_value_f 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_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) = 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; + 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_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_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})); From d638af737b2f7e40c939b9f93e93e55252a1d4b1 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 14 Jan 2025 21:02:28 +0100 Subject: [PATCH 4/8] Fix casting --- .../include/semantic_components/gps_sensor.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp index fe85d3ab12..6da44e229d 100644 --- a/controller_interface/include/semantic_components/gps_sensor.hpp +++ b/controller_interface/include/semantic_components/gps_sensor.hpp @@ -58,7 +58,10 @@ class GPSSensor : public SemanticComponentInterface * * \return Service */ - int8_t get_service() const { return static_cast(state_interfaces_[1].get().get_value()); } + uint16_t get_service() const + { + return static_cast(state_interfaces_[1].get().get_value()); + } /** * Return latitude reported by a GPS From 165c3c3f02876081ddb00eb71c55ad4058a62dde Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Tue, 14 Jan 2025 22:15:21 +0100 Subject: [PATCH 5/8] Make covariance always available --- .../include/semantic_components/gps_sensor.hpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp index 6da44e229d..8ecef93590 100644 --- a/controller_interface/include/semantic_components/gps_sensor.hpp +++ b/controller_interface/include/semantic_components/gps_sensor.hpp @@ -42,7 +42,6 @@ class GPSSensor : public SemanticComponentInterface interface_names_.emplace_back(name + "/" + "latitude_covariance"); interface_names_.emplace_back(name + "/" + "longitude_covariance"); interface_names_.emplace_back(name + "/" + "altitude_covariance"); - covariance_.fill(0.0); } } @@ -118,11 +117,7 @@ class GPSSensor : public SemanticComponentInterface } private: - struct Empty - { - }; - using CovarianceArray = std::conditional_t, Empty>; - CovarianceArray covariance_; + std::array covariance_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; }; } // namespace semantic_components From a0525028249edc324bf660c3cbf2b4d7527577fb Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sat, 18 Jan 2025 14:57:46 +0100 Subject: [PATCH 6/8] Add GPSSensorOption enum --- .../include/semantic_components/gps_sensor.hpp | 17 ++++++++++++----- controller_interface/test/test_gps_sensor.cpp | 10 +++++----- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp index 8ecef93590..6277113b4c 100644 --- a/controller_interface/include/semantic_components/gps_sensor.hpp +++ b/controller_interface/include/semantic_components/gps_sensor.hpp @@ -17,7 +17,6 @@ #include #include -#include #include "semantic_components/semantic_component_interface.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" @@ -25,7 +24,13 @@ namespace semantic_components { -template +enum class GPSSensorOption +{ + WithCovariance, + WithoutCovariance +}; + +template class GPSSensor : public SemanticComponentInterface { public: @@ -37,7 +42,7 @@ class GPSSensor : public SemanticComponentInterface {name + "/" + "longitude"}, {name + "/" + "altitude"}}) { - if constexpr (use_covariance) + if constexpr (sensor_option == GPSSensorOption::WithCovariance) { interface_names_.emplace_back(name + "/" + "latitude_covariance"); interface_names_.emplace_back(name + "/" + "longitude_covariance"); @@ -88,7 +93,9 @@ class GPSSensor : public SemanticComponentInterface * * \return Covariance array. */ - template > + template < + typename U = void, + typename = std::enable_if_t> const std::array & get_covariance() { covariance_[0] = state_interfaces_[5].get().get_value(); @@ -108,7 +115,7 @@ class GPSSensor : public SemanticComponentInterface message.longitude = get_longitude(); message.altitude = get_altitude(); - if constexpr (use_covariance) + if constexpr (sensor_option == GPSSensorOption::WithCovariance) { message.position_covariance = get_covariance(); } diff --git a/controller_interface/test/test_gps_sensor.cpp b/controller_interface/test/test_gps_sensor.cpp index 5377e18ad6..48ccc20526 100644 --- a/controller_interface/test/test_gps_sensor.cpp +++ b/controller_interface/test/test_gps_sensor.cpp @@ -16,8 +16,8 @@ #include #include #include -#include #include +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "semantic_components/gps_sensor.hpp" @@ -42,8 +42,8 @@ struct GPSSensorTest : public testing::Test const std::array gps_interface_names{ {"status", "service", "latitude", "longitude", "altitude"}}; std::array gps_states{}; - static constexpr bool use_covariance{false}; - semantic_components::GPSSensor sut{gps_sensor_name}; + semantic_components::GPSSensor sut{ + gps_sensor_name}; std::vector full_interface_names; hardware_interface::StateInterface gps_state{ @@ -140,8 +140,8 @@ struct GPSSensorWithCovarianceTest : public testing::Test {"status", "service", "latitude", "longitude", "altitude", "latitude_covariance", "longitude_covariance", "altitude_covariance"}}; std::array gps_states{}; - static constexpr bool use_covariance{true}; - semantic_components::GPSSensor sut{gps_sensor_name}; + semantic_components::GPSSensor sut{ + gps_sensor_name}; std::vector full_interface_names; hardware_interface::StateInterface gps_state{ From 8d45e013d4232e1d22adaf674c53e1952d576603 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 19 Jan 2025 01:17:08 +0000 Subject: [PATCH 7/8] Add static assert to check GPSSensorOption --- .../include/semantic_components/gps_sensor.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp index 6277113b4c..b6e255e427 100644 --- a/controller_interface/include/semantic_components/gps_sensor.hpp +++ b/controller_interface/include/semantic_components/gps_sensor.hpp @@ -42,6 +42,10 @@ class GPSSensor : public SemanticComponentInterface {name + "/" + "longitude"}, {name + "/" + "altitude"}}) { + static_assert( + sensor_option == GPSSensorOption::WithCovariance || + sensor_option == GPSSensorOption::WithoutCovariance, + "Invalid GPSSensorOption"); if constexpr (sensor_option == GPSSensorOption::WithCovariance) { interface_names_.emplace_back(name + "/" + "latitude_covariance"); From 469214d3156845b96eebaa707e82e66b1f0fc5d7 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 19 Jan 2025 20:32:31 +0000 Subject: [PATCH 8/8] Move static_assert from ctor --- .../include/semantic_components/gps_sensor.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/controller_interface/include/semantic_components/gps_sensor.hpp b/controller_interface/include/semantic_components/gps_sensor.hpp index b6e255e427..4abc943115 100644 --- a/controller_interface/include/semantic_components/gps_sensor.hpp +++ b/controller_interface/include/semantic_components/gps_sensor.hpp @@ -34,6 +34,10 @@ template class GPSSensor : public SemanticComponentInterface { public: + static_assert( + sensor_option == GPSSensorOption::WithCovariance || + sensor_option == GPSSensorOption::WithoutCovariance, + "Invalid GPSSensorOption"); explicit GPSSensor(const std::string & name) : SemanticComponentInterface( name, {{name + "/" + "status"}, @@ -42,10 +46,6 @@ class GPSSensor : public SemanticComponentInterface {name + "/" + "longitude"}, {name + "/" + "altitude"}}) { - static_assert( - sensor_option == GPSSensorOption::WithCovariance || - sensor_option == GPSSensorOption::WithoutCovariance, - "Invalid GPSSensorOption"); if constexpr (sensor_option == GPSSensorOption::WithCovariance) { interface_names_.emplace_back(name + "/" + "latitude_covariance");