From 37df4a333e46284f71ab3f591c70d435163980f4 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 23 Feb 2025 14:00:08 +0000 Subject: [PATCH 1/4] Add initial version of the GPSBroadcaster --- gps_sensor_broadcaster/CHANGELOG.rst | 219 ++++++++++++++++ gps_sensor_broadcaster/CMakeLists.txt | 101 ++++++++ gps_sensor_broadcaster/README.md | 8 + gps_sensor_broadcaster/doc/userdoc.rst | 27 ++ .../gps_sensor_broadcaster.xml | 8 + .../gps_sensor_broadcaster.hpp | 69 +++++ gps_sensor_broadcaster/package.xml | 37 +++ .../src/gps_sensor_broadcaster.cpp | 168 +++++++++++++ .../gps_sensor_broadcaster_parameters.yaml | 32 +++ .../test/gps_sensor_broadcaster_params.yaml | 5 + .../test/test_gps_sensor_broadcaster.cpp | 237 ++++++++++++++++++ .../test/test_load_gps_sensor_broadcaster.cpp | 46 ++++ ros2_controllers/package.xml | 1 + 13 files changed, 958 insertions(+) create mode 100644 gps_sensor_broadcaster/CHANGELOG.rst create mode 100644 gps_sensor_broadcaster/CMakeLists.txt create mode 100644 gps_sensor_broadcaster/README.md create mode 100644 gps_sensor_broadcaster/doc/userdoc.rst create mode 100644 gps_sensor_broadcaster/gps_sensor_broadcaster.xml create mode 100644 gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp create mode 100644 gps_sensor_broadcaster/package.xml create mode 100644 gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp create mode 100644 gps_sensor_broadcaster/src/gps_sensor_broadcaster_parameters.yaml create mode 100644 gps_sensor_broadcaster/test/gps_sensor_broadcaster_params.yaml create mode 100644 gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp create mode 100644 gps_sensor_broadcaster/test/test_load_gps_sensor_broadcaster.cpp diff --git a/gps_sensor_broadcaster/CHANGELOG.rst b/gps_sensor_broadcaster/CHANGELOG.rst new file mode 100644 index 0000000000..377ac195f5 --- /dev/null +++ b/gps_sensor_broadcaster/CHANGELOG.rst @@ -0,0 +1,219 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gpio_controllers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +4.20.0 (2025-01-29) +------------------- + +4.19.0 (2025-01-13) +------------------- + +4.18.0 (2024-12-19) +------------------- + +4.17.0 (2024-12-07) +------------------- + +4.16.0 (2024-11-08) +------------------- + +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + +4.12.0 (2024-07-23) +------------------- + +4.11.0 (2024-07-09) +------------------- + +4.10.0 (2024-07-01) +------------------- + +4.9.0 (2024-06-05) +------------------ + +4.8.0 (2024-05-14) +------------------ + +4.7.0 (2024-03-22) +------------------ + +4.6.0 (2024-02-12) +------------------ + +4.5.0 (2024-01-31) +------------------ + +4.4.0 (2024-01-11) +------------------ + +4.3.0 (2024-01-08) +------------------ + +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-12-01) +------------------ + +4.0.0 (2023-11-21) +------------------ + +3.17.0 (2023-10-31) +------------------- + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/gps_sensor_broadcaster/CMakeLists.txt b/gps_sensor_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..3c0a394c7e --- /dev/null +++ b/gps_sensor_broadcaster/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 3.16) +project(gps_sensor_broadcaster LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") +add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct + -Werror=missing-braces) +endif() + +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + + +generate_parameter_library(gps_sensor_broadcaster_parameters + src/gps_sensor_broadcaster_parameters.yaml +) + +add_library(gps_sensor_broadcaster SHARED + src/gps_sensor_broadcaster.cpp +) + +target_compile_features(gps_sensor_broadcaster PUBLIC cxx_std_17) +target_include_directories(gps_sensor_broadcaster PUBLIC + $ + $ +) + +target_link_libraries(gps_sensor_broadcaster PUBLIC gps_sensor_broadcaster_parameters) +ament_target_dependencies(gps_sensor_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +pluginlib_export_plugin_description_file( + controller_interface gps_sensor_broadcaster.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_gps_sensor_broadcaster + test/test_load_gps_sensor_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/gps_sensor_broadcaster_params.yaml) + target_include_directories(test_load_gps_sensor_broadcaster PRIVATE include) + target_link_libraries(test_load_gps_sensor_broadcaster + gps_sensor_broadcaster + ) + ament_target_dependencies(test_load_gps_sensor_broadcaster + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock(test_gps_sensor_broadcaster + test/test_gps_sensor_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/gps_sensor_broadcaster_params.yaml) + target_include_directories(test_gps_sensor_broadcaster PRIVATE include) + target_link_libraries(test_gps_sensor_broadcaster + gps_sensor_broadcaster + ) + ament_target_dependencies(test_gps_sensor_broadcaster + hardware_interface + ros2_control_test_assets + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/gps_sensor_broadcaster +) +install( + TARGETS + gps_sensor_broadcaster + gps_sensor_broadcaster_parameters + EXPORT export_gps_sensor_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +ament_export_targets(export_gps_sensor_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/gps_sensor_broadcaster/README.md b/gps_sensor_broadcaster/README.md new file mode 100644 index 0000000000..28203576f2 --- /dev/null +++ b/gps_sensor_broadcaster/README.md @@ -0,0 +1,8 @@ +gps_sensor_broadcaster +========================================== + +Controller to publish readings of GPS sensors. + +Pluginlib-Library: gps_sensor_broadcaster + +Plugin: gps_sensor_broadcaster/GPSSensorBroadcaster (controller_interface::ControllerInterface) diff --git a/gps_sensor_broadcaster/doc/userdoc.rst b/gps_sensor_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..2f0ab010fa --- /dev/null +++ b/gps_sensor_broadcaster/doc/userdoc.rst @@ -0,0 +1,27 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/gps_sensor_broadcaster/doc/userdoc.rst + +.. _gps_sensor_broadcaster_userdoc: + +GPS Sensor Broadcaster +-------------------------------- +Broadcaster of messages from GPS sensor. +The published message type is ``sensor_msgs/msg/NavSatFix``. + +The controller is a wrapper around ``GPSSensor`` semantic component (see ``controller_interface`` package). + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/gps_sensor_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/gps_sensor_broadcaster_params.yaml + :language: yaml diff --git a/gps_sensor_broadcaster/gps_sensor_broadcaster.xml b/gps_sensor_broadcaster/gps_sensor_broadcaster.xml new file mode 100644 index 0000000000..f18c027efe --- /dev/null +++ b/gps_sensor_broadcaster/gps_sensor_broadcaster.xml @@ -0,0 +1,8 @@ + + + + This controller publishes the readings of an GPS sensor as sensor_msgs/NavSatFix message. + + + diff --git a/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp b/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp new file mode 100644 index 0000000000..34306a0eca --- /dev/null +++ b/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp @@ -0,0 +1,69 @@ +// 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. + +/* + * Authors: Wiktor Bajor, Jakub Delicat + */ + +#ifndef GPS_SENSOR_BROADCASTER__GPS_SENSOR_BROADCASTER_HPP_ +#define GPS_SENSOR_BROADCASTER__GPS_SENSOR_BROADCASTER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "gps_sensor_broadcaster/gps_sensor_broadcaster_parameters.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "semantic_components/gps_sensor.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +namespace gps_sensor_broadcaster +{ +using callback_return_type = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +class GPSSensorBroadcaster : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + callback_return_type on_init() override; + callback_return_type on_configure(const rclcpp_lifecycle::State & previous_state) override; + callback_return_type on_activate(const rclcpp_lifecycle::State & previous_state) override; + callback_return_type on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::return_type update(const rclcpp::Time &, const rclcpp::Duration &) override; + +protected: + using GPSSensorOption = semantic_components::GPSSensorOption; + using GPSSensorVariant = std::variant< + std::monostate, semantic_components::GPSSensor, + semantic_components::GPSSensor>; + using StatePublisher = realtime_tools::RealtimePublisher; + + void setup_covariance(); + callback_return_type setup_publisher(); + + GPSSensorVariant gps_sensor_; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; + std::unique_ptr realtime_publisher_; + std::shared_ptr param_listener_{}; + gps_sensor_broadcaster::Params params_; + std::vector state_names; +}; + +} // namespace gps_sensor_broadcaster + +#endif // GPS_SENSOR_BROADCASTER__GPS_SENSOR_BROADCASTER_HPP_ diff --git a/gps_sensor_broadcaster/package.xml b/gps_sensor_broadcaster/package.xml new file mode 100644 index 0000000000..43e68576dd --- /dev/null +++ b/gps_sensor_broadcaster/package.xml @@ -0,0 +1,37 @@ + + + + gps_sensor_broadcaster + 4.20.0 + Controller to publish readings of GPS sensors. + Apache License 2.0 + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Wiktor Bajor + Jakub Delicat + + ament_cmake + + controller_interface + generate_parameter_library + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + sensor_msgs + + ament_cmake_gmock + ament_lint_auto + ament_lint_common + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp new file mode 100644 index 0000000000..c95266c1d4 --- /dev/null +++ b/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp @@ -0,0 +1,168 @@ +// 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. + +/* + * Authors: Wiktor Bajor, Jakub Delicat + */ + +#include "gps_sensor_broadcaster/gps_sensor_broadcaster.hpp" +#include + +namespace +{ +template +struct Visitor : Ts... +{ + using Ts::operator()...; +}; +template +Visitor(Ts...) -> Visitor; +constexpr std::size_t COVARIANCE_3x3_SIZE = 9; +constexpr uint16_t COVARIANCE_TYPE_KNOWN = 3; +constexpr uint16_t COVARIANCE_TYPE_DIAGONAL_KNOWN = 2; +} // namespace + +namespace gps_sensor_broadcaster +{ + +callback_return_type GPSSensorBroadcaster::on_init() +try +{ + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + return CallbackReturn::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; +} + +callback_return_type GPSSensorBroadcaster::on_configure(const rclcpp_lifecycle::State &) +{ + param_listener_->refresh_dynamic_parameters(); + params_ = param_listener_->get_params(); + + gps_sensor_ = + params_.read_covariance_from_interface + ? GPSSensorVariant{semantic_components::GPSSensor( + params_.sensor_name)} + : GPSSensorVariant{ + semantic_components::GPSSensor(params_.sensor_name)}; + std::visit( + Visitor{ + [this](auto & sensor) { state_names = sensor.get_state_interface_names(); }, + [](std::monostate &) {}}, + gps_sensor_); + + return setup_publisher(); +} + +callback_return_type GPSSensorBroadcaster::setup_publisher() +try +{ + sensor_state_publisher_ = get_node()->create_publisher( + "~/gps/fix", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(sensor_state_publisher_); + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + setup_covariance(); + realtime_publisher_->unlock(); + + return callback_return_type::SUCCESS; +} +catch (const std::exception & e) +{ + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message: %s \n", + e.what()); + return callback_return_type::ERROR; +} + +void GPSSensorBroadcaster::setup_covariance() +{ + if (params_.read_covariance_from_interface) + { + realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_DIAGONAL_KNOWN; + return; + } + + for (size_t i = 0; i < COVARIANCE_3x3_SIZE; ++i) + { + realtime_publisher_->msg_.position_covariance[i] = params_.static_position_covariance[i]; + realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_KNOWN; + } +} + +controller_interface::InterfaceConfiguration GPSSensorBroadcaster::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration GPSSensorBroadcaster::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = state_names; + + return state_interfaces_config; +} + +callback_return_type GPSSensorBroadcaster::on_activate(const rclcpp_lifecycle::State &) +{ + std::visit( + Visitor{ + [this](auto & sensor) { sensor.assign_loaned_state_interfaces(state_interfaces_); }, + [](std::monostate &) {}}, + gps_sensor_); + return callback_return_type::SUCCESS; +} + +callback_return_type GPSSensorBroadcaster::on_deactivate(const rclcpp_lifecycle::State &) +{ + std::visit( + Visitor{[](auto & sensor) { sensor.release_interfaces(); }, [](std::monostate &) {}}, + gps_sensor_); + return callback_return_type::SUCCESS; +} + +controller_interface::return_type GPSSensorBroadcaster::update( + const rclcpp::Time &, const rclcpp::Duration &) +{ + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = get_node()->now(); + std::visit( + Visitor{ + [this](auto & sensor) + { + sensor_msgs::msg::NavSatFix message; + sensor.get_values_as_message(realtime_publisher_->msg_); + }, + [](std::monostate &) {}}, + gps_sensor_); + realtime_publisher_->unlockAndPublish(); + } + return controller_interface::return_type::OK; +} +} // namespace gps_sensor_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + gps_sensor_broadcaster::GPSSensorBroadcaster, controller_interface::ControllerInterface) diff --git a/gps_sensor_broadcaster/src/gps_sensor_broadcaster_parameters.yaml b/gps_sensor_broadcaster/src/gps_sensor_broadcaster_parameters.yaml new file mode 100644 index 0000000000..f729e34c91 --- /dev/null +++ b/gps_sensor_broadcaster/src/gps_sensor_broadcaster_parameters.yaml @@ -0,0 +1,32 @@ +gps_sensor_broadcaster: + sensor_name: { + type: string, + default_value: "", + description: "Defines sensor name used as prefix for its interfaces. + Interface names are: ``/orientation.x, ..., /angular_velocity.x, ..., + /linear_acceleration.x.``", + validation: { + not_empty<>: null + } + } + frame_id: { + type: string, + default_value: "", + description: "Sensor's frame_id in which values are published.", + validation: { + not_empty<>: null + } + } + static_position_covariance: { + type: double_array, + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + description: "Static position covariance.", + validation: { + fixed_size<>: [9], + } + } + read_covariance_from_interface: { + type: bool, + default_value: False, + description: "Read covariance from state interface", + } diff --git a/gps_sensor_broadcaster/test/gps_sensor_broadcaster_params.yaml b/gps_sensor_broadcaster/test/gps_sensor_broadcaster_params.yaml new file mode 100644 index 0000000000..018f58bb94 --- /dev/null +++ b/gps_sensor_broadcaster/test/gps_sensor_broadcaster_params.yaml @@ -0,0 +1,5 @@ +test_gps_sensor_broadcaster: + ros__parameters: + sensor_name: gps_sensor + frame_id: gps_sensor_frame + service: service_gps diff --git a/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp new file mode 100644 index 0000000000..d0e6f885d9 --- /dev/null +++ b/gps_sensor_broadcaster/test/test_gps_sensor_broadcaster.cpp @@ -0,0 +1,237 @@ +// 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. + +/* + * Authors: Wiktor Bajor, Jakub Delicat + */ + +#include + +#include +#include +#include +#include "gmock/gmock.h" +#include "gps_sensor_broadcaster/gps_sensor_broadcaster.hpp" +#include "gps_sensor_broadcaster/gps_sensor_broadcaster_parameters.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +using hardware_interface::LoanedStateInterface; +using callback_return_type = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +namespace +{ +constexpr uint16_t GPS_SERVICE = 1; +constexpr uint16_t COVARIANCE_TYPE_KNOWN = 3; +constexpr uint16_t COVARIANCE_TYPE_DIAGONAL_KNOWN = 2; +rclcpp::WaitResultKind wait_for(std::shared_ptr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + return wait_set.wait().kind(); +} + +rclcpp::NodeOptions create_node_options_with_overriden_parameters( + std::vector parameters) +{ + auto node_options = rclcpp::NodeOptions(); + node_options.parameter_overrides(parameters); + return node_options; +} +} // namespace + +class GPSSensorBroadcasterTest : public ::testing::Test +{ +public: + GPSSensorBroadcasterTest() { rclcpp::init(0, nullptr); } + + ~GPSSensorBroadcasterTest() { rclcpp::shutdown(); } + + template < + semantic_components::GPSSensorOption sensor_option = + semantic_components::GPSSensorOption::WithoutCovariance> + void setup_gps_broadcaster() + { + std::vector state_ifs; + state_ifs.emplace_back(gps_status_); + state_ifs.emplace_back(gps_service_); + state_ifs.emplace_back(gps_latitude_); + state_ifs.emplace_back(gps_longitude_); + state_ifs.emplace_back(gps_altitude_); + if constexpr (sensor_option == semantic_components::GPSSensorOption::WithCovariance) + { + state_ifs.emplace_back(latitude_covariance_); + state_ifs.emplace_back(longitude_covariance_); + state_ifs.emplace_back(altitude_covariance_); + } + + gps_broadcaster_.assign_interfaces({}, std::move(state_ifs)); + } + + sensor_msgs::msg::NavSatFix subscribe_and_get_message() + { + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subscription = test_subscription_node.create_subscription( + "/test_gps_sensor_broadcaster/gps/fix", 10, + [](const sensor_msgs::msg::NavSatFix::SharedPtr) {}); + gps_broadcaster_.update(rclcpp::Time{}, rclcpp::Duration::from_seconds(0)); + wait_for(subscription); + + rclcpp::MessageInfo msg_info; + sensor_msgs::msg::NavSatFix gps_msg; + subscription->take(gps_msg, msg_info); + return gps_msg; + } + +protected: + const rclcpp::Parameter sensor_name_param_ = rclcpp::Parameter("sensor_name", "gps_sensor"); + const std::string sensor_name_ = sensor_name_param_.get_value(); + const rclcpp::Parameter frame_id_ = rclcpp::Parameter("frame_id", "gps_sensor_frame"); + std::array sensor_values_ = {{1.0, 1.0, 1.1, 2.2, 3.3, 0.5, 0.7, 0.9}}; + hardware_interface::StateInterface gps_status_{sensor_name_, "status", &sensor_values_[0]}; + hardware_interface::StateInterface gps_service_{sensor_name_, "service", &sensor_values_[1]}; + hardware_interface::StateInterface gps_latitude_{sensor_name_, "latitude", &sensor_values_[2]}; + hardware_interface::StateInterface gps_longitude_{sensor_name_, "longitude", &sensor_values_[3]}; + hardware_interface::StateInterface gps_altitude_{sensor_name_, "altitude", &sensor_values_[4]}; + hardware_interface::StateInterface latitude_covariance_{ + sensor_name_, "latitude_covariance", &sensor_values_[5]}; + hardware_interface::StateInterface longitude_covariance_{ + sensor_name_, "longitude_covariance", &sensor_values_[6]}; + hardware_interface::StateInterface altitude_covariance_{ + sensor_name_, "altitude_covariance", &sensor_values_[7]}; + + gps_sensor_broadcaster::GPSSensorBroadcaster gps_broadcaster_; +}; + +TEST_F(GPSSensorBroadcasterTest, whenNoParamsAreSetThenInitShouldFail) +{ + const auto result = gps_broadcaster_.init( + "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", + gps_broadcaster_.define_custom_node_options()); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F(GPSSensorBroadcasterTest, whenOnlySensorNameIsSetThenInitShouldFail) +{ + const auto node_options = create_node_options_with_overriden_parameters({sensor_name_param_}); + const auto result = gps_broadcaster_.init( + "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::ERROR); +} + +TEST_F( + GPSSensorBroadcasterTest, + whenAllRequiredArgumentsAreSetThenInitConfigureAndActivationShouldSucceed) +{ + const auto node_options = + create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_}); + const auto result = gps_broadcaster_.init( + "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ( + gps_broadcaster_.on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + ASSERT_EQ(gps_broadcaster_.on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); +} + +TEST_F( + GPSSensorBroadcasterTest, whenBroadcasterIsActiveShouldPublishNavSatMsgWithCovarianceSetToZero) +{ + const auto node_options = + create_node_options_with_overriden_parameters({sensor_name_param_, frame_id_}); + const auto result = gps_broadcaster_.init( + "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ( + gps_broadcaster_.on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + setup_gps_broadcaster(); + ASSERT_EQ(gps_broadcaster_.on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + const auto gps_msg = subscribe_and_get_message(); + EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); + EXPECT_EQ(gps_msg.status.status, sensor_values_[0]); + EXPECT_EQ(gps_msg.status.service, sensor_values_[1]); + EXPECT_EQ(gps_msg.latitude, sensor_values_[2]); + EXPECT_EQ(gps_msg.longitude, sensor_values_[3]); + EXPECT_EQ(gps_msg.altitude, sensor_values_[4]); + + const std::array expected_covariance = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + ASSERT_THAT(gps_msg.position_covariance, ::testing::ElementsAreArray(expected_covariance)); +} + +TEST_F( + GPSSensorBroadcasterTest, + whenBroadcasterIsActiveAndStaticCovarianceShouldPublishNavSatMsgWithStaticCovariance) +{ + const std::array static_covariance = {{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}}; + const auto node_options = create_node_options_with_overriden_parameters( + {sensor_name_param_, + frame_id_, + {"static_position_covariance", + std::vector{static_covariance.begin(), static_covariance.end()}}}); + const auto result = gps_broadcaster_.init( + "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ( + gps_broadcaster_.on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + setup_gps_broadcaster(); + ASSERT_EQ(gps_broadcaster_.on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + const auto gps_msg = subscribe_and_get_message(); + EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); + EXPECT_EQ(gps_msg.status.status, sensor_values_[0]); + EXPECT_EQ(gps_msg.status.service, sensor_values_[1]); + EXPECT_EQ(gps_msg.latitude, sensor_values_[2]); + EXPECT_EQ(gps_msg.longitude, sensor_values_[3]); + EXPECT_EQ(gps_msg.altitude, sensor_values_[4]); + + ASSERT_THAT(gps_msg.position_covariance, ::testing::ElementsAreArray(static_covariance)); + ASSERT_THAT(gps_msg.position_covariance_type, COVARIANCE_TYPE_KNOWN); +} + +TEST_F( + GPSSensorBroadcasterTest, + whenBroadcasterReadsCovarianceFormInterfaceThenValusesShouldBroadcastedInMsg) +{ + const auto node_options = create_node_options_with_overriden_parameters( + {sensor_name_param_, frame_id_, {"read_covariance_from_interface", true}}); + const auto result = gps_broadcaster_.init( + "test_gps_sensor_broadcaster", ros2_control_test_assets::minimal_robot_urdf, 0, "", + node_options); + ASSERT_EQ(result, controller_interface::return_type::OK); + ASSERT_EQ( + gps_broadcaster_.on_configure(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + setup_gps_broadcaster(); + ASSERT_EQ(gps_broadcaster_.on_activate(rclcpp_lifecycle::State()), callback_return_type::SUCCESS); + + const auto gps_msg = subscribe_and_get_message(); + EXPECT_EQ(gps_msg.header.frame_id, frame_id_.get_value()); + EXPECT_EQ(gps_msg.status.status, sensor_values_[0]); + EXPECT_EQ(gps_msg.status.service, sensor_values_[1]); + EXPECT_EQ(gps_msg.latitude, sensor_values_[2]); + EXPECT_EQ(gps_msg.longitude, sensor_values_[3]); + EXPECT_EQ(gps_msg.altitude, sensor_values_[4]); + + const std::array expected_covariance = { + {sensor_values_[5], 0.0, 0.0, 0.0, sensor_values_[6], 0.0, 0.0, 0.0, sensor_values_[7]}}; + ASSERT_THAT(gps_msg.position_covariance, ::testing::ElementsAreArray(expected_covariance)); + ASSERT_THAT(gps_msg.position_covariance_type, COVARIANCE_TYPE_DIAGONAL_KNOWN); +} diff --git a/gps_sensor_broadcaster/test/test_load_gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/test/test_load_gps_sensor_broadcaster.cpp new file mode 100644 index 0000000000..b0b47e20b0 --- /dev/null +++ b/gps_sensor_broadcaster/test/test_load_gps_sensor_broadcaster.cpp @@ -0,0 +1,46 @@ +// 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. + +/* + * Authors: Wiktor Bajor, Jakub Delicat + */ + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGPSSensorBroadcaster, load_controller) +{ + rclcpp::init(0, nullptr); + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/gps_sensor_broadcaster_params.yaml"; + + cm.set_parameter({"test_gps_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_gps_sensor_broadcaster.type", "gps_sensor_broadcaster/GPSSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_gps_sensor_broadcaster"), nullptr); + rclcpp::shutdown(); +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 4495828bff..c36755244e 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -25,6 +25,7 @@ force_torque_sensor_broadcaster forward_command_controller gpio_controllers + gps_sensor_broadcaster gripper_controllers imu_sensor_broadcaster joint_state_broadcaster From d21ae078a66a6eaeb97ca2260575d5bbe4738127 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Mon, 24 Feb 2025 21:04:40 +0000 Subject: [PATCH 2/4] Review fixes --- gps_sensor_broadcaster/CHANGELOG.rst | 219 ------------------ .../gps_sensor_broadcaster.hpp | 2 +- .../src/gps_sensor_broadcaster.cpp | 60 ++--- 3 files changed, 33 insertions(+), 248 deletions(-) delete mode 100644 gps_sensor_broadcaster/CHANGELOG.rst diff --git a/gps_sensor_broadcaster/CHANGELOG.rst b/gps_sensor_broadcaster/CHANGELOG.rst deleted file mode 100644 index 377ac195f5..0000000000 --- a/gps_sensor_broadcaster/CHANGELOG.rst +++ /dev/null @@ -1,219 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package gpio_controllers -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -4.20.0 (2025-01-29) -------------------- - -4.19.0 (2025-01-13) -------------------- - -4.18.0 (2024-12-19) -------------------- - -4.17.0 (2024-12-07) -------------------- - -4.16.0 (2024-11-08) -------------------- - -4.15.0 (2024-10-07) -------------------- - -4.14.0 (2024-09-11) -------------------- - -4.13.0 (2024-08-22) -------------------- - -4.12.1 (2024-08-14) -------------------- - -4.12.0 (2024-07-23) -------------------- - -4.11.0 (2024-07-09) -------------------- - -4.10.0 (2024-07-01) -------------------- - -4.9.0 (2024-06-05) ------------------- - -4.8.0 (2024-05-14) ------------------- - -4.7.0 (2024-03-22) ------------------- - -4.6.0 (2024-02-12) ------------------- - -4.5.0 (2024-01-31) ------------------- - -4.4.0 (2024-01-11) ------------------- - -4.3.0 (2024-01-08) ------------------- - -4.2.0 (2023-12-12) ------------------- - -4.1.0 (2023-12-01) ------------------- - -4.0.0 (2023-11-21) ------------------- - -3.17.0 (2023-10-31) -------------------- - -3.16.0 (2023-09-20) -------------------- - -3.15.0 (2023-09-11) -------------------- - -3.14.0 (2023-08-16) -------------------- - -3.13.0 (2023-08-04) -------------------- - -3.12.0 (2023-07-18) -------------------- - -3.11.0 (2023-06-24) -------------------- - -3.10.1 (2023-06-06) -------------------- - -3.10.0 (2023-06-04) -------------------- - -3.9.0 (2023-05-28) ------------------- - -3.8.0 (2023-05-14) ------------------- - -3.7.0 (2023-05-02) ------------------- - -3.6.0 (2023-04-29) ------------------- - -3.5.0 (2023-04-14) ------------------- - -3.4.0 (2023-04-02) ------------------- - -3.3.0 (2023-03-07) ------------------- - -3.2.0 (2023-02-10) ------------------- - -3.1.0 (2023-01-26) ------------------- - -3.0.0 (2023-01-19) ------------------- - -2.15.0 (2022-12-06) -------------------- - -2.14.0 (2022-11-18) -------------------- - -2.13.0 (2022-10-05) -------------------- - -2.12.0 (2022-09-01) -------------------- - -2.11.0 (2022-08-04) -------------------- - -2.10.0 (2022-08-01) -------------------- - -2.9.0 (2022-07-14) ------------------- - -2.8.0 (2022-07-09) ------------------- - -2.7.0 (2022-07-03) ------------------- - -2.6.0 (2022-06-18) ------------------- - -2.5.0 (2022-05-13) ------------------- - -2.4.0 (2022-04-29) ------------------- - -2.3.0 (2022-04-21) ------------------- - -2.2.0 (2022-03-25) ------------------- - -2.1.0 (2022-02-23) ------------------- - -2.0.1 (2022-02-01) ------------------- - -2.0.0 (2022-01-28) ------------------- - -1.3.0 (2022-01-11) ------------------- - -1.2.0 (2021-12-29) ------------------- - -1.1.0 (2021-10-25) ------------------- - -1.0.0 (2021-09-29) ------------------- - -0.5.0 (2021-08-30) ------------------- - -0.4.1 (2021-07-08) ------------------- - -0.4.0 (2021-06-28) ------------------- - -0.3.1 (2021-05-23) ------------------- - -0.3.0 (2021-05-21) ------------------- - -0.2.1 (2021-05-03) ------------------- - -0.2.0 (2021-02-06) ------------------- - -0.1.2 (2021-01-07) ------------------- - -0.1.1 (2021-01-06) ------------------- - -0.1.0 (2020-12-23) ------------------- diff --git a/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp b/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp index 34306a0eca..35c88073aa 100644 --- a/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp +++ b/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp @@ -61,7 +61,7 @@ class GPSSensorBroadcaster : public controller_interface::ControllerInterface std::unique_ptr realtime_publisher_; std::shared_ptr param_listener_{}; gps_sensor_broadcaster::Params params_; - std::vector state_names; + std::vector state_names_; }; } // namespace gps_sensor_broadcaster diff --git a/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp index c95266c1d4..182eb0e87d 100644 --- a/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp @@ -37,16 +37,18 @@ namespace gps_sensor_broadcaster { callback_return_type GPSSensorBroadcaster::on_init() -try { - param_listener_ = std::make_shared(get_node()); - params_ = param_listener_->get_params(); - return CallbackReturn::SUCCESS; -} -catch (const std::exception & e) -{ - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + return CallbackReturn::SUCCESS; + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } } callback_return_type GPSSensorBroadcaster::on_configure(const rclcpp_lifecycle::State &) @@ -62,7 +64,7 @@ callback_return_type GPSSensorBroadcaster::on_configure(const rclcpp_lifecycle:: semantic_components::GPSSensor(params_.sensor_name)}; std::visit( Visitor{ - [this](auto & sensor) { state_names = sensor.get_state_interface_names(); }, + [this](auto & sensor) { state_names_ = sensor.get_state_interface_names(); }, [](std::monostate &) {}}, gps_sensor_); @@ -70,24 +72,26 @@ callback_return_type GPSSensorBroadcaster::on_configure(const rclcpp_lifecycle:: } callback_return_type GPSSensorBroadcaster::setup_publisher() -try { - sensor_state_publisher_ = get_node()->create_publisher( - "~/gps/fix", rclcpp::SystemDefaultsQoS()); - realtime_publisher_ = std::make_unique(sensor_state_publisher_); - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; - setup_covariance(); - realtime_publisher_->unlock(); - - return callback_return_type::SUCCESS; -} -catch (const std::exception & e) -{ - fprintf( - stderr, "Exception thrown during publisher creation at configure stage with message: %s \n", - e.what()); - return callback_return_type::ERROR; + try + { + sensor_state_publisher_ = get_node()->create_publisher( + "~/gps/fix", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = std::make_unique(sensor_state_publisher_); + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + setup_covariance(); + realtime_publisher_->unlock(); + + return callback_return_type::SUCCESS; + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message: %s \n", + e.what()); + return callback_return_type::ERROR; + } } void GPSSensorBroadcaster::setup_covariance() @@ -118,7 +122,7 @@ controller_interface::InterfaceConfiguration GPSSensorBroadcaster::state_interfa { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names = state_names; + state_interfaces_config.names = state_names_; return state_interfaces_config; } From 2f109ae31ecc13394cc02c01db47fdcf402e8c3a Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Mon, 31 Mar 2025 09:36:39 +0000 Subject: [PATCH 3/4] Update the cmake and user doc --- doc/controllers_index.rst | 2 +- gps_sensor_broadcaster/CMakeLists.txt | 8 +++----- gps_sensor_broadcaster/package.xml | 1 + 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index fe09839ce5..6fd3b159b2 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -74,7 +74,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> - + GPS Sensor Broadcaster <../gps_sensor_broadcaster/doc/userdoc.rst> Common Controller Parameters **************************** diff --git a/gps_sensor_broadcaster/CMakeLists.txt b/gps_sensor_broadcaster/CMakeLists.txt index 3c0a394c7e..80928a223d 100644 --- a/gps_sensor_broadcaster/CMakeLists.txt +++ b/gps_sensor_broadcaster/CMakeLists.txt @@ -1,11 +1,9 @@ cmake_minimum_required(VERSION 3.16) project(gps_sensor_broadcaster LANGUAGES CXX) -if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") -add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable - -Werror=return-type -Werror=shadow -Werror=format -Werror=range-loop-construct - -Werror=missing-braces) -endif() +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() # using this instead of visibility macros # S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 diff --git a/gps_sensor_broadcaster/package.xml b/gps_sensor_broadcaster/package.xml index 43e68576dd..e6ec14f2a5 100644 --- a/gps_sensor_broadcaster/package.xml +++ b/gps_sensor_broadcaster/package.xml @@ -24,6 +24,7 @@ rclcpp_lifecycle realtime_tools sensor_msgs + ros2_control_cmake ament_cmake_gmock ament_lint_auto From ba43e471e65ecf90facc15247bb44006e90ff7ca Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Mon, 31 Mar 2025 09:44:52 +0000 Subject: [PATCH 4/4] Move cmake ros2 control to build deps --- gps_sensor_broadcaster/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gps_sensor_broadcaster/package.xml b/gps_sensor_broadcaster/package.xml index e6ec14f2a5..b819648628 100644 --- a/gps_sensor_broadcaster/package.xml +++ b/gps_sensor_broadcaster/package.xml @@ -15,6 +15,7 @@ Jakub Delicat ament_cmake + ros2_control_cmake controller_interface generate_parameter_library @@ -24,7 +25,6 @@ rclcpp_lifecycle realtime_tools sensor_msgs - ros2_control_cmake ament_cmake_gmock ament_lint_auto