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..4abc943115 --- /dev/null +++ b/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 +#include + +#include "semantic_components/semantic_component_interface.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +namespace semantic_components +{ + +enum class GPSSensorOption +{ + WithCovariance, + WithoutCovariance +}; + +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"}, + {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(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(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> + const std::array & 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 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_ diff --git a/controller_interface/test/test_gps_sensor.cpp b/controller_interface/test/test_gps_sensor.cpp new file mode 100644 index 0000000000..48ccc20526 --- /dev/null +++ b/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 +#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(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", "service", "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 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 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 gps_interface_names{ + {"status", "service", "latitude", "longitude", "altitude", "latitude_covariance", + "longitude_covariance", "altitude_covariance"}}; + 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 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 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})); +}