From d4139b7a4943844b7e61ef59a3d81d8d8beb4baa Mon Sep 17 00:00:00 2001 From: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Date: Mon, 20 Jan 2025 21:04:57 +0100 Subject: [PATCH 1/2] Add GPS semantic component (#2000) --- controller_interface/CMakeLists.txt | 6 + .../semantic_components/gps_sensor.hpp | 136 ++++++++++ controller_interface/test/test_gps_sensor.cpp | 239 ++++++++++++++++++ 3 files changed, 381 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..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})); +} From fbfc01d84de1ed4f02b39ba2d77ab492c0afea0b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 23 Jan 2025 12:42:09 +0100 Subject: [PATCH 2/2] Use `target_compile_definitions` instead of installing test files (#2009) --- controller_interface/CMakeLists.txt | 5 +- .../test/test_controller_with_options.cpp | 9 ++- controller_manager/CMakeLists.txt | 12 +--- controller_manager/package.xml | 1 - .../test/test_spawner_unspawner.cpp | 65 +++++++++---------- 5 files changed, 41 insertions(+), 51 deletions(-) diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index dc810eaeb5..10604a5255 100644 --- a/controller_interface/CMakeLists.txt +++ b/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 diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index eb60c19aca..80b6c5cf3b 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -16,7 +16,6 @@ #include #include -#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( diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 5d91604cec..e704393022 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -11,7 +11,6 @@ endif() set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) set(THIS_PACKAGE_INCLUDE_DEPENDS - ament_index_cpp controller_interface controller_manager_msgs diagnostic_updater @@ -201,6 +200,9 @@ if(BUILD_TESTING) test_controller ros2_control_test_assets::ros2_control_test_assets ) + target_compile_definitions( + test_spawner_unspawner + PRIVATE PARAMETERS_FILE_PATH="${CMAKE_CURRENT_LIST_DIR}/test/") ament_add_gmock(test_hardware_spawner test/test_hardware_spawner.cpp @@ -212,14 +214,6 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) - install(FILES - test/test_controller_spawner_with_type.yaml - test/test_controller_overriding_parameters.yaml - test/test_controller_spawner_with_fallback_controllers.yaml - test/test_controller_spawner_wildcard_entries.yaml - test/test_controller_spawner_with_interfaces.yaml - DESTINATION test) - ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp ) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 8e1a9b261e..61f9bceb55 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -12,7 +12,6 @@ ament_cmake_gen_version_h ament_cmake_python - ament_index_cpp backward_ros controller_interface controller_manager_msgs diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index ff39b69e8c..3afcdfefb9 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -257,8 +257,8 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); cm_->set_parameter(rclcpp::Parameter( "ctrl_with_parameters_and_type.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); @@ -301,8 +301,8 @@ TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) TEST_F(TestLoadController, spawner_test_type_in_params_file) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file @@ -368,8 +368,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_wildcard_entries.yaml"; + const std::string test_file_path = std::string(PARAMETERS_FILE_PATH) + + std::string("test_controller_spawner_wildcard_entries.yaml"); ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file @@ -439,8 +439,8 @@ TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name) TEST_F(TestLoadController, spawner_test_failed_activation_of_controllers) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_interfaces.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_interfaces.yaml"); ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file @@ -577,11 +577,9 @@ TEST_F(TestLoadController, unload_on_kill_activate_as_group) TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding) { const std::string main_test_file_path = - ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); const std::string overriding_test_file_path = - ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_overriding_parameters.yaml"; + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_overriding_parameters.yaml"); ControllerManagerRunner cm_runner(this); EXPECT_EQ( @@ -631,11 +629,9 @@ TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding) TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding_reverse) { const std::string main_test_file_path = - ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_overriding_parameters.yaml"; + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_overriding_parameters.yaml"); const std::string overriding_test_file_path = - ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); ControllerManagerRunner cm_runner(this); EXPECT_EQ( @@ -684,8 +680,9 @@ TEST_F(TestLoadController, spawner_test_to_check_parameter_overriding_reverse) TEST_F(TestLoadController, spawner_test_fallback_controllers) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_fallback_controllers.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + + std::string("test_controller_spawner_with_fallback_controllers.yaml"); cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); @@ -1031,8 +1028,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file @@ -1106,8 +1103,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) TEST_F( TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file_deprecated_namespace_arg) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file @@ -1196,8 +1193,8 @@ TEST_F( TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file @@ -1263,8 +1260,8 @@ TEST_F( TestLoadControllerWithNamespacedCM, spawner_test_fail_namespaced_controllers_with_non_wildcard_entries) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file @@ -1303,11 +1300,11 @@ TEST_F( TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); const std::string fallback_test_file_path = - ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_fallback_controllers.yaml"; + std::string(PARAMETERS_FILE_PATH) + + std::string("test_controller_spawner_with_fallback_controllers.yaml"); cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); @@ -1370,11 +1367,11 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) { - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_type.yaml"; + const std::string test_file_path = + std::string(PARAMETERS_FILE_PATH) + std::string("test_controller_spawner_with_type.yaml"); const std::string fallback_test_file_path = - ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_fallback_controllers.yaml"; + std::string(PARAMETERS_FILE_PATH) + + std::string("test_controller_spawner_with_fallback_controllers.yaml"); cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));