From 493129468b55078a8292e681587f76aea6e3d2a4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 9 Nov 2025 02:02:19 +0100 Subject: [PATCH 1/7] Add initial commit of generic state broadcaster --- generic_state_broadcaster/CMakeLists.txt | 88 +++++++++++ .../generic_state_broadcaster_plugin.xml | 7 + .../generic_state_broadcaster.hpp | 84 +++++++++++ generic_state_broadcaster/package.xml | 43 ++++++ .../src/generic_state_broadcaster.cpp | 137 ++++++++++++++++++ .../generic_state_broadcaster_parameters.yaml | 10 ++ 6 files changed, 369 insertions(+) create mode 100644 generic_state_broadcaster/CMakeLists.txt create mode 100644 generic_state_broadcaster/generic_state_broadcaster_plugin.xml create mode 100644 generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp create mode 100644 generic_state_broadcaster/package.xml create mode 100644 generic_state_broadcaster/src/generic_state_broadcaster.cpp create mode 100644 generic_state_broadcaster/src/generic_state_broadcaster_parameters.yaml diff --git a/generic_state_broadcaster/CMakeLists.txt b/generic_state_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..5eaba096fb --- /dev/null +++ b/generic_state_broadcaster/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.16) +project(generic_state_broadcaster) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + builtin_interfaces + control_msgs + controller_interface + generate_parameter_library + pluginlib + rclcpp_lifecycle + realtime_tools +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(generic_state_broadcaster_parameters + src/generic_state_broadcaster_parameters.yaml +) + +add_library(generic_state_broadcaster SHARED + src/generic_state_broadcaster.cpp +) +target_compile_features(generic_state_broadcaster PUBLIC cxx_std_17) +target_include_directories(generic_state_broadcaster PUBLIC + $ + $ +) +target_link_libraries(generic_state_broadcaster PUBLIC + generic_state_broadcaster_parameters + controller_interface::controller_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${control_msgs_TARGETS} + ${builtin_interfaces_TARGETS}) +pluginlib_export_plugin_description_file(controller_interface generic_state_broadcaster_plugin.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(rclcpp REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + # ament_add_gmock(test_load_generic_state_broadcaster + # test/test_load_generic_state_broadcaster.cpp + # ) + # target_link_libraries(test_load_generic_state_broadcaster + # generic_state_broadcaster + # controller_manager::controller_manager + # hardware_interface::hardware_interface + # rclcpp::rclcpp + # ros2_control_test_assets::ros2_control_test_assets) + + # ament_add_gmock(test_generic_state_broadcaster + # test/test_generic_state_broadcaster.cpp + # ) + # target_link_libraries(test_generic_state_broadcaster + # generic_state_broadcaster + # ros2_control_test_assets::ros2_control_test_assets) +endif() + +install( + DIRECTORY include/ + DESTINATION include/generic_state_broadcaster +) +install( + TARGETS + generic_state_broadcaster + generic_state_broadcaster_parameters + EXPORT export_generic_state_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_generic_state_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/generic_state_broadcaster/generic_state_broadcaster_plugin.xml b/generic_state_broadcaster/generic_state_broadcaster_plugin.xml new file mode 100644 index 0000000000..ac9bdf1b94 --- /dev/null +++ b/generic_state_broadcaster/generic_state_broadcaster_plugin.xml @@ -0,0 +1,7 @@ + + + + The generic state broadcaster publishes the values of the requested interfaces from ros2_control system. + + + diff --git a/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp b/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp new file mode 100644 index 0000000000..126d79b232 --- /dev/null +++ b/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp @@ -0,0 +1,84 @@ +// Copyright (c) 2025, PAL Robotics +// +// 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 GENERIC_STATE_BROADCASTER__GENERIC_STATE_BROADCASTER_HPP_ +#define GENERIC_STATE_BROADCASTER__GENERIC_STATE_BROADCASTER_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/msg/dynamic_joint_state.hpp" +#include "control_msgs/msg/string_array.hpp" +#include "control_msgs/msg/values_array.hpp" +#include "controller_interface/controller_interface.hpp" +#include "realtime_tools/realtime_publisher.hpp" + +// auto-generated by generate_parameter_library +#include "generic_state_broadcaster/generic_state_broadcaster_parameters.hpp" + +namespace generic_state_broadcaster +{ +/** + * \brief Generic State Broadcaster for selected state interfaces in a ros2_control system. + * + * GenericStateBroadcaster publishes the selected state interfaces from ros2_control as ROS + * messages. + * + * Publishes to: + * - \b ~/names (control_msgs::msg::StringArray): The list of the interface names that are selected + * to be published by the generic state broadcaster. This is published with transient local + * durability. + * - \b ~/values (control_msgs::msg::ValuesArray): The list of the values corresponding to the + * interface names that are selected to be published by the generic state broadcaster. + * + * \note The values are published at the same rate as the controller update rate. + */ +class GenericStateBroadcaster : public controller_interface::ControllerInterface +{ +public: + GenericStateBroadcaster(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + // Optional parameters + std::shared_ptr param_listener_; + Params params_; + + // publishers and messages + std::shared_ptr> names_publisher_; + std::shared_ptr> values_publisher_; + std::shared_ptr> + realtime_values_publisher_; + control_msgs::msg::ValuesArray values_msg_; +}; + +} // namespace generic_state_broadcaster + +#endif // GENERIC_STATE_BROADCASTER__GENERIC_STATE_BROADCASTER_HPP_ diff --git a/generic_state_broadcaster/package.xml b/generic_state_broadcaster/package.xml new file mode 100644 index 0000000000..c997ed9c79 --- /dev/null +++ b/generic_state_broadcaster/package.xml @@ -0,0 +1,43 @@ + + + generic_state_broadcaster + 5.8.0 + Broadcaster to publish desired hardware interface states + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + ament_cmake + + ros2_control_cmake + + backward_ros + builtin_interfaces + control_msgs + controller_interface + generate_parameter_library + pluginlib + rclcpp_lifecycle + rclcpp + realtime_tools + + ament_cmake_gmock + controller_manager + hardware_interface_testing + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/generic_state_broadcaster/src/generic_state_broadcaster.cpp b/generic_state_broadcaster/src/generic_state_broadcaster.cpp new file mode 100644 index 0000000000..1d27fb1217 --- /dev/null +++ b/generic_state_broadcaster/src/generic_state_broadcaster.cpp @@ -0,0 +1,137 @@ +// Copyright (c) 2025, PAL Robotics +// +// 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 "generic_state_broadcaster/generic_state_broadcaster.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/time.hpp" + +namespace generic_state_broadcaster +{ + +GenericStateBroadcaster::GenericStateBroadcaster() {} + +controller_interface::CallbackReturn GenericStateBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +GenericStateBroadcaster::command_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration +GenericStateBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : params_.interfaces) + { + state_interfaces_config.names.push_back(interface); + } + return state_interfaces_config; +} + +controller_interface::CallbackReturn GenericStateBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + names_publisher_ = get_node()->create_publisher( + "~/names", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); + values_publisher_ = get_node()->create_publisher( + "~/values", rclcpp::SystemDefaultsQoS()); + realtime_values_publisher_ = + std::make_shared>( + values_publisher_); + + values_msg_.values.clear(); + values_msg_.values.resize(params_.interfaces.size(), std::numeric_limits::quiet_NaN()); + + control_msgs::msg::StringArray names_msg; + names_msg.names = params_.interfaces; + names_msg.header.stamp = get_node()->now(); + names_publisher_->publish(names_msg); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GenericStateBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for (auto i = 0u; i < state_interfaces_.size(); ++i) + { + if (!state_interfaces_[i].is_castable_to_double()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "State interface '%s' is not castable to double. The GenericStateBroadcaster only supports " + "state interfaces that can be casted to double.", + params_.interfaces[i].c_str()); + return CallbackReturn::FAILURE; + } + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type GenericStateBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + for (auto i = 0u; i < state_interfaces_.size(); ++i) + { + const auto & opt = state_interfaces_[i].get_optional(); + if (opt.has_value()) + { + values_msg_.values[i] = opt.value(); + } + } + + if (realtime_values_publisher_) + { + values_msg_.header.stamp = time; + realtime_values_publisher_->try_publish(values_msg_); + } + + return controller_interface::return_type::OK; +} + +} // namespace generic_state_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + generic_state_broadcaster::GenericStateBroadcaster, controller_interface::ControllerInterface) diff --git a/generic_state_broadcaster/src/generic_state_broadcaster_parameters.yaml b/generic_state_broadcaster/src/generic_state_broadcaster_parameters.yaml new file mode 100644 index 0000000000..7f23e806d9 --- /dev/null +++ b/generic_state_broadcaster/src/generic_state_broadcaster_parameters.yaml @@ -0,0 +1,10 @@ +generic_state_broadcaster: + interfaces: { + type: string_array, + default_value: [], + description: "The list of hardware interfaces information to be published by the generic state broadcaster.", + read_only: true, + validation: { + not_empty<>: [] + } + } From 6875e65fa6a48b8aac341ff2ae82efa87d6fc2af Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 9 Nov 2025 23:13:11 +0100 Subject: [PATCH 2/7] Add first version of the tests --- generic_state_broadcaster/CMakeLists.txt | 30 +- .../src/generic_state_broadcaster.cpp | 2 +- .../test/test_generic_state_broadcaster.cpp | 685 ++++++++++++++++++ .../test/test_generic_state_broadcaster.hpp | 124 ++++ .../test_load_generic_state_broadcaster.cpp | 41 ++ 5 files changed, 866 insertions(+), 16 deletions(-) create mode 100644 generic_state_broadcaster/test/test_generic_state_broadcaster.cpp create mode 100644 generic_state_broadcaster/test/test_generic_state_broadcaster.hpp create mode 100644 generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp diff --git a/generic_state_broadcaster/CMakeLists.txt b/generic_state_broadcaster/CMakeLists.txt index 5eaba096fb..c2c5709841 100644 --- a/generic_state_broadcaster/CMakeLists.txt +++ b/generic_state_broadcaster/CMakeLists.txt @@ -51,22 +51,22 @@ if(BUILD_TESTING) find_package(rclcpp REQUIRED) find_package(ros2_control_test_assets REQUIRED) - # ament_add_gmock(test_load_generic_state_broadcaster - # test/test_load_generic_state_broadcaster.cpp - # ) - # target_link_libraries(test_load_generic_state_broadcaster - # generic_state_broadcaster - # controller_manager::controller_manager - # hardware_interface::hardware_interface - # rclcpp::rclcpp - # ros2_control_test_assets::ros2_control_test_assets) + ament_add_gmock(test_load_generic_state_broadcaster + test/test_load_generic_state_broadcaster.cpp + ) + target_link_libraries(test_load_generic_state_broadcaster + generic_state_broadcaster + controller_manager::controller_manager + hardware_interface::hardware_interface + rclcpp::rclcpp + ros2_control_test_assets::ros2_control_test_assets) - # ament_add_gmock(test_generic_state_broadcaster - # test/test_generic_state_broadcaster.cpp - # ) - # target_link_libraries(test_generic_state_broadcaster - # generic_state_broadcaster - # ros2_control_test_assets::ros2_control_test_assets) + ament_add_gmock(test_generic_state_broadcaster + test/test_generic_state_broadcaster.cpp + ) + target_link_libraries(test_generic_state_broadcaster + generic_state_broadcaster + ros2_control_test_assets::ros2_control_test_assets) endif() install( diff --git a/generic_state_broadcaster/src/generic_state_broadcaster.cpp b/generic_state_broadcaster/src/generic_state_broadcaster.cpp index 1d27fb1217..5874889586 100644 --- a/generic_state_broadcaster/src/generic_state_broadcaster.cpp +++ b/generic_state_broadcaster/src/generic_state_broadcaster.cpp @@ -43,7 +43,7 @@ controller_interface::CallbackReturn GenericStateBroadcaster::on_init() return CallbackReturn::ERROR; } - return CallbackReturn::SUCCESS; + return params_.interfaces.empty() ? CallbackReturn::ERROR : CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration diff --git a/generic_state_broadcaster/test/test_generic_state_broadcaster.cpp b/generic_state_broadcaster/test/test_generic_state_broadcaster.cpp new file mode 100644 index 0000000000..1a21197919 --- /dev/null +++ b/generic_state_broadcaster/test/test_generic_state_broadcaster.cpp @@ -0,0 +1,685 @@ +// Copyright (c) 2025, PAL Robotics +// +// 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 "gmock/gmock.h" + +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "test_generic_state_broadcaster.hpp" + +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::LoanedStateInterface; +using std::placeholders::_1; +using testing::Each; +using testing::ElementsAreArray; +using testing::IsEmpty; +using testing::SizeIs; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace + +void GenericStateBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void GenericStateBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } + +void GenericStateBroadcasterTest::SetUp() +{ + // initialize broadcaster + state_broadcaster_ = std::make_unique(); +} + +void GenericStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); } + +controller_interface::return_type GenericStateBroadcasterTest::SetUpStateBroadcaster(const std::vector & interfaces) +{ + RCLCPP_INFO( + rclcpp::get_logger("GenericStateBroadcasterTest"), + "Setting up GenericStateBroadcaster with interfaces: %d", + static_cast(interfaces.size())); + auto result = init_broadcaster_and_set_parameters("", interfaces); + if(result == controller_interface::return_type::OK) + { + assign_state_interfaces(interfaces); + } + return result; +} + +controller_interface::return_type GenericStateBroadcasterTest::init_broadcaster_and_set_parameters( + const std::string & robot_description, const std::vector & interfaces) +{ + controller_interface::ControllerInterfaceParams params; + params.controller_name = "joint_state_broadcaster"; + params.robot_description = robot_description; + params.update_rate = 0; + params.node_namespace = ""; + params.node_options = state_broadcaster_->define_custom_node_options(); + if(!interfaces.empty()) + { + params.node_options.parameter_overrides({rclcpp::Parameter("interfaces", rclcpp::ParameterValue(interfaces))}); + } + return state_broadcaster_->init(params); +} + +void GenericStateBroadcasterTest::assign_state_interfaces(const std::vector & interfaces) +{ + std::vector state_ifs; + if (interfaces.empty()) + { + state_ifs.emplace_back(joint_1_pos_state_); + state_ifs.emplace_back(joint_2_pos_state_); + state_ifs.emplace_back(joint_3_pos_state_); + state_ifs.emplace_back(joint_1_vel_state_); + state_ifs.emplace_back(joint_2_vel_state_); + state_ifs.emplace_back(joint_3_vel_state_); + state_ifs.emplace_back(joint_1_eff_state_); + state_ifs.emplace_back(joint_2_eff_state_); + state_ifs.emplace_back(joint_3_eff_state_); + } + else + { + for (const auto & interface : interfaces) + { + RCLCPP_INFO( + state_broadcaster_->get_node()->get_logger(), "Assigning interface: %s", interface.c_str()); + if (interface == joint_names_[0] + "/" + interface_names_[0]) + { + state_ifs.emplace_back(joint_1_pos_state_); + } + if (interface == joint_names_[1] + "/" + interface_names_[0]) + { + state_ifs.emplace_back(joint_2_pos_state_); + } + if (interface == joint_names_[2] + "/" + interface_names_[0]) + { + state_ifs.emplace_back(joint_3_pos_state_); + } + if (interface == joint_names_[0] + "/" + interface_names_[1]) + { + state_ifs.emplace_back(joint_1_vel_state_); + } + if (interface == joint_names_[1] + "/" + interface_names_[1]) + { + state_ifs.emplace_back(joint_2_vel_state_); + } + if (interface == joint_names_[2] + "/" + interface_names_[1]) + { + state_ifs.emplace_back(joint_3_vel_state_); + } + if (interface == joint_names_[0] + "/" + interface_names_[2]) + { + state_ifs.emplace_back(joint_1_eff_state_); + } + if (interface == joint_names_[1] + "/" + interface_names_[2]) + { + state_ifs.emplace_back(joint_2_eff_state_); + } + if (interface == joint_names_[2] + "/" + interface_names_[2]) + { + state_ifs.emplace_back(joint_3_eff_state_); + } + if (interface == custom_interface_name_) + { + state_ifs.emplace_back(joint_X_custom_state); + } + } + } + + state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); +} + +TEST_F(GenericStateBroadcasterTest, FailOnEmptyInterfaceListTest) +{ + // publishers not initialized yet + ASSERT_FALSE(state_broadcaster_->names_publisher_); + ASSERT_FALSE(state_broadcaster_->values_publisher_); + + ASSERT_EQ(SetUpStateBroadcaster({}), controller_interface::return_type::ERROR); +} + +TEST_F(GenericStateBroadcasterTest, ConfigureOnValidInterfaceListTest) +{ + // publishers not initialized yet + ASSERT_FALSE(state_broadcaster_->names_publisher_); + ASSERT_FALSE(state_broadcaster_->values_publisher_); + + const std::vector interfaces = { + joint_names_[0] + "/" + interface_names_[0], + joint_names_[1] + "/" + interface_names_[1], + joint_names_[2] + "/" + interface_names_[2], + }; + ASSERT_EQ(SetUpStateBroadcaster(interfaces), controller_interface::return_type::OK); + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(state_broadcaster_->names_publisher_); + ASSERT_TRUE(state_broadcaster_->values_publisher_); + + ASSERT_EQ(state_broadcaster_->values_msg_.values.size(), 3); + ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[0])); + ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[1])); + ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[2])); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(state_broadcaster_->values_msg_.values.size(), 3); + ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[0])); + ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[1])); + ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[2])); + + ASSERT_EQ(state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); +} + + +// TEST_F(GenericStateBroadcasterTest, TestCustomInterfaceWithoutMapping) +// { +// const std::vector JOINT_NAMES = {joint_names_[0]}; +// const std::vector IF_NAMES = {custom_interface_name_}; +// SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + +// // configure ok +// ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// const size_t NUM_JOINTS = JOINT_NAMES.size(); + +// // check interface configuration +// auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); +// ASSERT_THAT(cmd_if_conf.names, IsEmpty()); +// EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); +// auto state_if_conf = state_broadcaster_->state_interface_configuration(); +// ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); +// EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + +// // joint state initialized +// const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; +// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); +// ASSERT_THAT(joint_state_msg.name, SizeIs(0)); +// ASSERT_THAT(joint_state_msg.position, SizeIs(0)); +// ASSERT_THAT(joint_state_msg.velocity, SizeIs(0)); +// ASSERT_THAT(joint_state_msg.effort, SizeIs(0)); + +// // dynamic joint state initialized +// const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; +// ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); +// ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); +// ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); +// ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); +// ASSERT_THAT( +// dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); + +// // publishers initialized +// ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); +// ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +// } + +// TEST_F(GenericStateBroadcasterTest, TestCustomInterfaceMapping) +// { +// const std::vector JOINT_NAMES = {joint_names_[0]}; +// const std::vector IF_NAMES = {custom_interface_name_}; +// SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + +// state_broadcaster_->get_node()->set_parameter( +// {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); + +// // configure ok +// ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// const size_t NUM_JOINTS = JOINT_NAMES.size(); + +// // check interface configuration +// auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); +// ASSERT_THAT(cmd_if_conf.names, IsEmpty()); +// EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); +// auto state_if_conf = state_broadcaster_->state_interface_configuration(); +// ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); +// EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + +// // joint state initialized +// const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; +// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); +// ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); +// ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); +// ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); +// for (auto i = 0ul; i < NUM_JOINTS; ++i) +// { +// ASSERT_TRUE(std::isnan(joint_state_msg.velocity[i])); +// } +// ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); +// for (auto i = 0ul; i < NUM_JOINTS; ++i) +// { +// ASSERT_TRUE(std::isnan(joint_state_msg.effort[i])); +// } + +// // dynamic joint state initialized +// const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; +// ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); +// ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); +// ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); +// ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); +// ASSERT_THAT( +// dynamic_joint_state_msg.interface_values[0].interface_names, +// ElementsAreArray({HW_IF_POSITION})); // mapping to this value + +// // publishers initialized +// ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); +// ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +// } + +// TEST_F(GenericStateBroadcasterTest, TestCustomInterfaceMappingUpdate) +// { +// const std::vector JOINT_NAMES = {joint_names_[0]}; +// const std::vector IF_NAMES = {custom_interface_name_}; +// SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); + +// state_broadcaster_->get_node()->set_parameter( +// {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); + +// sensor_msgs::msg::JointState joint_state_msg; +// activate_and_get_joint_state_message("joint_states", joint_state_msg); + +// const size_t NUM_JOINTS = JOINT_NAMES.size(); + +// // joint state initialized +// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); +// ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); +// ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); +// ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_); +// ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); +// for (auto i = 0ul; i < NUM_JOINTS; ++i) +// { +// ASSERT_TRUE(std::isnan(joint_state_msg.velocity[i])); +// } +// ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); +// for (auto i = 0ul; i < NUM_JOINTS; ++i) +// { +// ASSERT_TRUE(std::isnan(joint_state_msg.effort[i])); +// } + +// // dynamic joint state initialized +// const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; +// ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); +// ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); +// ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); +// ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); +// ASSERT_THAT( +// dynamic_joint_state_msg.interface_values[0].interface_names, +// ElementsAreArray({HW_IF_POSITION})); + +// // publishers initialized +// ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); +// ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); +// } + +// TEST_F(GenericStateBroadcasterTest, UpdateTest) +// { +// SetUpStateBroadcaster(); + +// auto node_state = state_broadcaster_->configure(); +// node_state = state_broadcaster_->get_node()->activate(); +// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +// ASSERT_EQ( +// state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// } + +// TEST_F(GenericStateBroadcasterTest, UpdatePerformanceTest) +// { +// controller_interface::ControllerInterfaceParams params; +// params.controller_name = "joint_state_broadcaster"; +// params.robot_description = ""; +// params.update_rate = 0; +// params.node_namespace = ""; +// params.node_options = state_broadcaster_->define_custom_node_options(); +// const auto result = state_broadcaster_->init(params); +// ASSERT_EQ(result, controller_interface::return_type::OK); + +// custom_joint_value_ = 12.34; + +// // build our own test interfaces: robot has ~500 state interfaces +// for (auto joint = 1u; joint < 30; ++joint) +// { +// const auto joint_name = "joint_" + std::to_string(joint); + +// // standard +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "position", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "velocity", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "effort", &custom_joint_value_)); + +// // non standard +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "mode", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "absolute_position", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "acceleration", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "current", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "torque", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "force", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "temperature_board", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "temperature_motor", &custom_joint_value_)); + +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "position.kd", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "position.ki", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "position.kp", &custom_joint_value_)); + +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "velocity.kd", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "velocity.ki", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "velocity.kp", &custom_joint_value_)); + +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "current.kd", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "current.ki", &custom_joint_value_)); +// test_interfaces_.emplace_back( +// std::make_shared( +// joint_name, "current.kp", &custom_joint_value_)); +// } + +// RCLCPP_INFO( +// state_broadcaster_->get_node()->get_logger(), "Number of test interfaces: %zu", +// test_interfaces_.size()); + +// std::vector state_interfaces; +// for (const auto & tif : test_interfaces_) +// { +// state_interfaces.emplace_back(tif, nullptr); +// } + +// state_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); + +// auto node_state = state_broadcaster_->configure(); +// node_state = state_broadcaster_->get_node()->activate(); +// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + +// if (!realtime_tools::configure_sched_fifo(50)) +// { +// RCLCPP_WARN( +// state_broadcaster_->get_node()->get_logger(), +// "Could not enable FIFO RT scheduling policy: with error number <%i>(%s)", errno, +// strerror(errno)); +// } + +// constexpr auto kNumSamples = 10000u; +// std::vector measures; +// for (auto i = 0u; i < kNumSamples; ++i) +// { +// const auto now = std::chrono::steady_clock::now(); + +// ASSERT_EQ( +// state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// // print time taken +// const auto elapsed = std::chrono::steady_clock::now() - now; +// const auto elapsed_us = std::chrono::duration_cast(elapsed); + +// measures.push_back(elapsed_us.count()); +// } + +// const auto average = +// std::accumulate(measures.begin(), measures.end(), 0.0) / static_cast(measures.size()); +// const auto variance = std::accumulate( +// measures.begin(), measures.end(), 0.0, [average](double accum, double x) +// { return accum + (x - average) * (x - average); }) / +// static_cast(measures.size()); + +// RCLCPP_INFO(state_broadcaster_->get_node()->get_logger(), "Average update time: %lf us", average); +// RCLCPP_INFO(state_broadcaster_->get_node()->get_logger(), "Variance: %lf us", variance); +// } + +// void GenericStateBroadcasterTest::activate_and_get_joint_state_message( +// const std::string & topic, sensor_msgs::msg::JointState & joint_state_msg) +// { +// auto node_state = state_broadcaster_->configure(); +// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + +// node_state = state_broadcaster_->get_node()->activate(); +// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + +// sensor_msgs::msg::JointState::SharedPtr received_msg; +// rclcpp::Node test_node("test_node"); +// auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr msg) +// { received_msg = msg; }; +// auto subscription = +// test_node.create_subscription(topic, 10, subs_callback); + +// rclcpp::executors::SingleThreadedExecutor executor; +// executor.add_node(test_node.get_node_base_interface()); + +// // call update to publish the test value +// // since update doesn't guarantee a published message, republish until received +// int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop +// while (max_sub_check_loop_count--) +// { +// state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// const auto timeout = std::chrono::milliseconds{5}; +// const auto until = test_node.get_clock()->now() + timeout; +// while (!received_msg && test_node.get_clock()->now() < until) +// { +// executor.spin_some(); +// std::this_thread::sleep_for(std::chrono::microseconds(10)); +// } +// // check if message has been received +// if (received_msg.get()) +// { +// break; +// } +// } +// ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " +// "controller/broadcaster update loop"; +// ASSERT_TRUE(received_msg); + +// // take message from subscription +// joint_state_msg = *received_msg; +// } + +// void GenericStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) +// { +// sensor_msgs::msg::JointState joint_state_msg; +// activate_and_get_joint_state_message(topic, joint_state_msg); + +// const size_t NUM_JOINTS = joint_names_.size(); +// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); +// ASSERT_THAT(joint_state_msg.name, SizeIs(NUM_JOINTS)); +// // the order in the message may be different +// // we only check that all values in this test are present in the message +// // and that it is the same across the interfaces +// // for test purposes they are mapped to the same doubles +// ASSERT_THAT(joint_state_msg.position, ElementsAreArray(joint_values_)); +// ASSERT_THAT(joint_state_msg.velocity, ElementsAreArray(joint_state_msg.position)); +// ASSERT_THAT(joint_state_msg.effort, ElementsAreArray(joint_state_msg.position)); +// } + +// TEST_F(GenericStateBroadcasterTest, JointStatePublishTest) +// { +// SetUpStateBroadcaster(); + +// test_published_joint_state_message("joint_states"); +// } + +// TEST_F(GenericStateBroadcasterTest, JointStatePublishTestLocalTopic) +// { +// SetUpStateBroadcaster(); +// state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); + +// test_published_joint_state_message("joint_state_broadcaster/joint_states"); +// } + +// void GenericStateBroadcasterTest::test_published_dynamic_joint_state_message( +// const std::string & topic) +// { +// auto node_state = state_broadcaster_->configure(); +// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + +// node_state = state_broadcaster_->get_node()->activate(); +// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + +// control_msgs::msg::DynamicJointState::SharedPtr dynamic_joint_state_msg; +// rclcpp::Node test_node("test_node"); +// auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr msg) +// { dynamic_joint_state_msg = msg; }; +// auto subscription = +// test_node.create_subscription(topic, 10, subs_callback); +// rclcpp::executors::SingleThreadedExecutor executor; +// executor.add_node(test_node.get_node_base_interface()); +// dynamic_joint_state_msg.reset(); +// ASSERT_FALSE(dynamic_joint_state_msg); + +// // call update to publish the test value +// // since update doesn't guarantee a published message, republish until received +// int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop +// while (max_sub_check_loop_count--) +// { +// state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); +// const auto timeout = std::chrono::milliseconds{5}; +// const auto until = test_node.get_clock()->now() + timeout; +// while (test_node.get_clock()->now() < until) +// { +// executor.spin_some(); +// std::this_thread::sleep_for(std::chrono::microseconds(10)); +// } +// // check if message has been received +// if (dynamic_joint_state_msg.get()) +// { +// break; +// } +// } +// ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " +// "controller/broadcaster update loop"; +// ASSERT_TRUE(dynamic_joint_state_msg); + +// const size_t NUM_JOINTS = 3; +// const std::vector INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; +// ASSERT_EQ(dynamic_joint_state_msg->header.frame_id, frame_id_); +// ASSERT_THAT(dynamic_joint_state_msg->joint_names, SizeIs(NUM_JOINTS)); +// // the order in the message may be different +// // we only check that all values in this test are present in the message +// // and that it is the same across the interfaces +// // for test purposes they are mapped to the same doubles +// for (size_t i = 0; i < dynamic_joint_state_msg->joint_names.size(); ++i) +// { +// ASSERT_THAT( +// dynamic_joint_state_msg->interface_values[i].interface_names, +// ElementsAreArray(INTERFACE_NAMES)); +// const auto index_in_original_order = static_cast(std::distance( +// joint_names_.cbegin(), +// std::find( +// joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i]))); +// ASSERT_THAT( +// dynamic_joint_state_msg->interface_values[i].values, +// Each(joint_values_[index_in_original_order])); +// } +// } + +// TEST_F(GenericStateBroadcasterTest, DynamicJointStatePublishTest) +// { +// SetUpStateBroadcaster(); + +// test_published_dynamic_joint_state_message("dynamic_joint_states"); +// } + +// TEST_F(GenericStateBroadcasterTest, DynamicJointStatePublishTestLocalTopic) +// { +// SetUpStateBroadcaster(); +// state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); + +// test_published_dynamic_joint_state_message("joint_state_broadcaster/dynamic_joint_states"); +// } + +// TEST_F(GenericStateBroadcasterTest, ExtraJointStatePublishTest) +// { +// // publishers not initialized yet +// ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_); +// ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + +// SetUpStateBroadcaster(); + +// // Add extra joints as parameters +// const std::vector extra_joint_names = {"extra1", "extra2", "extra3"}; +// state_broadcaster_->get_node()->set_parameter({"extra_joints", extra_joint_names}); + +// // configure ok +// ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// std::vector all_joint_names = joint_names_; +// all_joint_names.insert(all_joint_names.end(), extra_joint_names.begin(), extra_joint_names.end()); +// const size_t NUM_JOINTS = all_joint_names.size(); + +// // joint state initialized +// const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; +// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); +// ASSERT_THAT(joint_state_msg.name, ElementsAreArray(all_joint_names)); +// ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); +// ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); +// ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + +// // dynamic joint state initialized +// const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; +// ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); +// ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); +// } diff --git a/generic_state_broadcaster/test/test_generic_state_broadcaster.hpp b/generic_state_broadcaster/test/test_generic_state_broadcaster.hpp new file mode 100644 index 0000000000..748e5a0e82 --- /dev/null +++ b/generic_state_broadcaster/test/test_generic_state_broadcaster.hpp @@ -0,0 +1,124 @@ +// Copyright (c) 2025, PAL Robotics +// +// 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 TEST_GENERIC_STATE_BROADCASTER_HPP_ +#define TEST_GENERIC_STATE_BROADCASTER_HPP_ + +#include + +#include +#include +#include + +#include "generic_state_broadcaster/generic_state_broadcaster.hpp" + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; + +// subclassing and friending so we can access member variables +class FriendGenericStateBroadcaster : public generic_state_broadcaster::GenericStateBroadcaster +{ + FRIEND_TEST(GenericStateBroadcasterTest, FailOnEmptyInterfaceListTest); + FRIEND_TEST(GenericStateBroadcasterTest, ConfigureOnValidInterfaceListTest); + FRIEND_TEST(GenericStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithoutJointsParameter); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithJointsAndInterfaces); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing); + FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); + FRIEND_TEST(GenericStateBroadcasterTest, TestCustomInterfaceWithoutMapping); + FRIEND_TEST(GenericStateBroadcasterTest, TestCustomInterfaceMapping); + FRIEND_TEST(GenericStateBroadcasterTest, TestCustomInterfaceMappingUpdate); + FRIEND_TEST(GenericStateBroadcasterTest, ExtraJointStatePublishTest); +}; + +class GenericStateBroadcasterTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + controller_interface::return_type SetUpStateBroadcaster( + const std::vector & interfaces = {}); + + controller_interface::return_type init_broadcaster_and_set_parameters( + const std::string & robot_description, const std::vector & interfaces); + + void assign_state_interfaces(const std::vector & interfaces = {}); + + void test_published_joint_state_message(const std::string & topic); + + void test_published_dynamic_joint_state_message(const std::string & topic); + + void activate_and_get_state_message( + const std::string & topic, control_msgs::msg::ValuesArray & msg); + +protected: + // dummy joint state values used for tests + const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + const std::vector interface_names_ = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; + std::string custom_interface_name_ = "measured_position"; + std::vector joint_values_ = {1.1, 2.1, 3.1}; + double custom_joint_value_ = 3.5; + + hardware_interface::StateInterface::SharedPtr joint_1_pos_state_ = + std::make_shared( + joint_names_[0], interface_names_[0], &joint_values_[0]); + hardware_interface::StateInterface::SharedPtr joint_2_pos_state_ = + std::make_shared( + joint_names_[1], interface_names_[0], &joint_values_[1]); + hardware_interface::StateInterface::SharedPtr joint_3_pos_state_ = + std::make_shared( + joint_names_[2], interface_names_[0], &joint_values_[2]); + hardware_interface::StateInterface::SharedPtr joint_1_vel_state_ = + std::make_shared( + joint_names_[0], interface_names_[1], &joint_values_[0]); + hardware_interface::StateInterface::SharedPtr joint_2_vel_state_ = + std::make_shared( + joint_names_[1], interface_names_[1], &joint_values_[1]); + hardware_interface::StateInterface::SharedPtr joint_3_vel_state_ = + std::make_shared( + joint_names_[2], interface_names_[1], &joint_values_[2]); + hardware_interface::StateInterface::SharedPtr joint_1_eff_state_ = + std::make_shared( + joint_names_[0], interface_names_[2], &joint_values_[0]); + hardware_interface::StateInterface::SharedPtr joint_2_eff_state_ = + std::make_shared( + joint_names_[1], interface_names_[2], &joint_values_[1]); + hardware_interface::StateInterface::SharedPtr joint_3_eff_state_ = + std::make_shared( + joint_names_[2], interface_names_[2], &joint_values_[2]); + + hardware_interface::StateInterface::SharedPtr joint_X_custom_state = + std::make_shared( + joint_names_[0], custom_interface_name_, &custom_joint_value_); + + std::vector test_interfaces_; + + std::unique_ptr state_broadcaster_; + std::string frame_id_ = "base_link"; +}; + +#endif // TEST_GENERIC_STATE_BROADCASTER_HPP_ diff --git a/generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp b/generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp new file mode 100644 index 0000000000..8b3725cd71 --- /dev/null +++ b/generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2025, PAL Robotics +// +// 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 "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(TestLoadGenericStateBroadcaster, 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"); + + ASSERT_NE( + cm.load_controller( + "test_generic_state_broadcaster", "generic_state_broadcaster/GenericStateBroadcaster"), + nullptr); + + rclcpp::shutdown(); +} From 7e8e34a0ae9b9535fa1e7e65a59642b946e8ef69 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Nov 2025 22:44:15 +0100 Subject: [PATCH 3/7] change to new message types --- .../generic_state_broadcaster.hpp | 12 ++++++------ .../src/generic_state_broadcaster.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp b/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp index 126d79b232..709c0c47fc 100644 --- a/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp +++ b/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp @@ -21,8 +21,8 @@ #include #include "control_msgs/msg/dynamic_joint_state.hpp" -#include "control_msgs/msg/string_array.hpp" -#include "control_msgs/msg/values_array.hpp" +#include "control_msgs/msg/interfaces_names.hpp" +#include "control_msgs/msg/interfaces_values.hpp" #include "controller_interface/controller_interface.hpp" #include "realtime_tools/realtime_publisher.hpp" @@ -72,11 +72,11 @@ class GenericStateBroadcaster : public controller_interface::ControllerInterface Params params_; // publishers and messages - std::shared_ptr> names_publisher_; - std::shared_ptr> values_publisher_; - std::shared_ptr> + std::shared_ptr> names_publisher_; + std::shared_ptr> values_publisher_; + std::shared_ptr> realtime_values_publisher_; - control_msgs::msg::ValuesArray values_msg_; + control_msgs::msg::InterfacesValues values_msg_; }; } // namespace generic_state_broadcaster diff --git a/generic_state_broadcaster/src/generic_state_broadcaster.cpp b/generic_state_broadcaster/src/generic_state_broadcaster.cpp index 5874889586..0e4f44b4fd 100644 --- a/generic_state_broadcaster/src/generic_state_broadcaster.cpp +++ b/generic_state_broadcaster/src/generic_state_broadcaster.cpp @@ -70,18 +70,18 @@ controller_interface::CallbackReturn GenericStateBroadcaster::on_configure( { params_ = param_listener_->get_params(); - names_publisher_ = get_node()->create_publisher( + names_publisher_ = get_node()->create_publisher( "~/names", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); - values_publisher_ = get_node()->create_publisher( + values_publisher_ = get_node()->create_publisher( "~/values", rclcpp::SystemDefaultsQoS()); realtime_values_publisher_ = - std::make_shared>( + std::make_shared>( values_publisher_); values_msg_.values.clear(); values_msg_.values.resize(params_.interfaces.size(), std::numeric_limits::quiet_NaN()); - control_msgs::msg::StringArray names_msg; + control_msgs::msg::InterfacesNames names_msg; names_msg.names = params_.interfaces; names_msg.header.stamp = get_node()->now(); names_publisher_->publish(names_msg); From d2a4b584e441a73f55711ecd82a00ccdd0e2d853 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Nov 2025 00:10:14 +0100 Subject: [PATCH 4/7] update tests --- .../generic_state_broadcaster.hpp | 1 + .../src/generic_state_broadcaster.cpp | 7 +- .../test/test_generic_state_broadcaster.cpp | 670 ++++-------------- .../test/test_generic_state_broadcaster.hpp | 18 +- 4 files changed, 138 insertions(+), 558 deletions(-) diff --git a/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp b/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp index 709c0c47fc..5c9f579cb2 100644 --- a/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp +++ b/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp @@ -77,6 +77,7 @@ class GenericStateBroadcaster : public controller_interface::ControllerInterface std::shared_ptr> realtime_values_publisher_; control_msgs::msg::InterfacesValues values_msg_; + control_msgs::msg::InterfacesNames names_msg_; }; } // namespace generic_state_broadcaster diff --git a/generic_state_broadcaster/src/generic_state_broadcaster.cpp b/generic_state_broadcaster/src/generic_state_broadcaster.cpp index 0e4f44b4fd..a897a81ca1 100644 --- a/generic_state_broadcaster/src/generic_state_broadcaster.cpp +++ b/generic_state_broadcaster/src/generic_state_broadcaster.cpp @@ -81,10 +81,9 @@ controller_interface::CallbackReturn GenericStateBroadcaster::on_configure( values_msg_.values.clear(); values_msg_.values.resize(params_.interfaces.size(), std::numeric_limits::quiet_NaN()); - control_msgs::msg::InterfacesNames names_msg; - names_msg.names = params_.interfaces; - names_msg.header.stamp = get_node()->now(); - names_publisher_->publish(names_msg); + names_msg_.names = params_.interfaces; + names_msg_.header.stamp = get_node()->now(); + names_publisher_->publish(names_msg_); return CallbackReturn::SUCCESS; } diff --git a/generic_state_broadcaster/test/test_generic_state_broadcaster.cpp b/generic_state_broadcaster/test/test_generic_state_broadcaster.cpp index 1a21197919..86f098729e 100644 --- a/generic_state_broadcaster/test/test_generic_state_broadcaster.cpp +++ b/generic_state_broadcaster/test/test_generic_state_broadcaster.cpp @@ -59,16 +59,16 @@ void GenericStateBroadcasterTest::SetUp() void GenericStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); } -controller_interface::return_type GenericStateBroadcasterTest::SetUpStateBroadcaster(const std::vector & interfaces) +controller_interface::return_type GenericStateBroadcasterTest::SetUpStateBroadcaster( + const std::vector & interfaces) { RCLCPP_INFO( rclcpp::get_logger("GenericStateBroadcasterTest"), - "Setting up GenericStateBroadcaster with interfaces: %d", - static_cast(interfaces.size())); + "Setting up GenericStateBroadcaster with interfaces: %d", static_cast(interfaces.size())); auto result = init_broadcaster_and_set_parameters("", interfaces); - if(result == controller_interface::return_type::OK) + if (result == controller_interface::return_type::OK) { - assign_state_interfaces(interfaces); + assign_state_interfaces(interfaces); } return result; } @@ -77,19 +77,21 @@ controller_interface::return_type GenericStateBroadcasterTest::init_broadcaster_ const std::string & robot_description, const std::vector & interfaces) { controller_interface::ControllerInterfaceParams params; - params.controller_name = "joint_state_broadcaster"; + params.controller_name = "generic_state_broadcaster"; params.robot_description = robot_description; params.update_rate = 0; params.node_namespace = ""; params.node_options = state_broadcaster_->define_custom_node_options(); - if(!interfaces.empty()) + if (!interfaces.empty()) { - params.node_options.parameter_overrides({rclcpp::Parameter("interfaces", rclcpp::ParameterValue(interfaces))}); + params.node_options.parameter_overrides( + {rclcpp::Parameter("interfaces", rclcpp::ParameterValue(interfaces))}); } return state_broadcaster_->init(params); } -void GenericStateBroadcasterTest::assign_state_interfaces(const std::vector & interfaces) +void GenericStateBroadcasterTest::assign_state_interfaces( + const std::vector & interfaces) { std::vector state_ifs; if (interfaces.empty()) @@ -106,56 +108,102 @@ void GenericStateBroadcasterTest::assign_state_interfaces(const std::vectorget_node()->get_logger(), "Assigning interface: %s", interface.c_str()); + if (interface == joint_names_[0] + "/" + interface_names_[0]) { - RCLCPP_INFO( - state_broadcaster_->get_node()->get_logger(), "Assigning interface: %s", interface.c_str()); - if (interface == joint_names_[0] + "/" + interface_names_[0]) - { - state_ifs.emplace_back(joint_1_pos_state_); - } - if (interface == joint_names_[1] + "/" + interface_names_[0]) - { - state_ifs.emplace_back(joint_2_pos_state_); - } - if (interface == joint_names_[2] + "/" + interface_names_[0]) - { - state_ifs.emplace_back(joint_3_pos_state_); - } - if (interface == joint_names_[0] + "/" + interface_names_[1]) - { - state_ifs.emplace_back(joint_1_vel_state_); - } - if (interface == joint_names_[1] + "/" + interface_names_[1]) - { - state_ifs.emplace_back(joint_2_vel_state_); - } - if (interface == joint_names_[2] + "/" + interface_names_[1]) - { - state_ifs.emplace_back(joint_3_vel_state_); - } - if (interface == joint_names_[0] + "/" + interface_names_[2]) - { - state_ifs.emplace_back(joint_1_eff_state_); - } - if (interface == joint_names_[1] + "/" + interface_names_[2]) - { - state_ifs.emplace_back(joint_2_eff_state_); - } - if (interface == joint_names_[2] + "/" + interface_names_[2]) - { - state_ifs.emplace_back(joint_3_eff_state_); - } - if (interface == custom_interface_name_) - { - state_ifs.emplace_back(joint_X_custom_state); - } + state_ifs.emplace_back(joint_1_pos_state_); } + if (interface == joint_names_[1] + "/" + interface_names_[0]) + { + state_ifs.emplace_back(joint_2_pos_state_); + } + if (interface == joint_names_[2] + "/" + interface_names_[0]) + { + state_ifs.emplace_back(joint_3_pos_state_); + } + if (interface == joint_names_[0] + "/" + interface_names_[1]) + { + state_ifs.emplace_back(joint_1_vel_state_); + } + if (interface == joint_names_[1] + "/" + interface_names_[1]) + { + state_ifs.emplace_back(joint_2_vel_state_); + } + if (interface == joint_names_[2] + "/" + interface_names_[1]) + { + state_ifs.emplace_back(joint_3_vel_state_); + } + if (interface == joint_names_[0] + "/" + interface_names_[2]) + { + state_ifs.emplace_back(joint_1_eff_state_); + } + if (interface == joint_names_[1] + "/" + interface_names_[2]) + { + state_ifs.emplace_back(joint_2_eff_state_); + } + if (interface == joint_names_[2] + "/" + interface_names_[2]) + { + state_ifs.emplace_back(joint_3_eff_state_); + } + if (interface == custom_interface_name_) + { + state_ifs.emplace_back(joint_X_custom_state); + } + } } state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } +void GenericStateBroadcasterTest::activate_and_get_state_message( + const std::string & topic, control_msgs::msg::InterfacesValues & msg) +{ + auto node_state = state_broadcaster_->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = state_broadcaster_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + control_msgs::msg::InterfacesValues::SharedPtr received_msg; + rclcpp::Node test_node("test_node"); + auto subs_callback = [&](const control_msgs::msg::InterfacesValues::SharedPtr cb_msg) + { received_msg = cb_msg; }; + auto subscription = + test_node.create_subscription(topic, 10, subs_callback); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node.get_node_base_interface()); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_node.get_clock()->now() + timeout; + while (!received_msg && test_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; +} + TEST_F(GenericStateBroadcasterTest, FailOnEmptyInterfaceListTest) { // publishers not initialized yet @@ -186,500 +234,46 @@ TEST_F(GenericStateBroadcasterTest, ConfigureOnValidInterfaceListTest) ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[0])); ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[1])); ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[2])); + ASSERT_THAT(state_broadcaster_->names_msg_.names, ElementsAreArray(interfaces)); ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(state_broadcaster_->values_msg_.values.size(), 3); ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[0])); ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[1])); ASSERT_TRUE(std::isnan(state_broadcaster_->values_msg_.values[2])); + ASSERT_THAT(state_broadcaster_->names_msg_.names, ElementsAreArray(interfaces)); - ASSERT_EQ(state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); -} - - -// TEST_F(GenericStateBroadcasterTest, TestCustomInterfaceWithoutMapping) -// { -// const std::vector JOINT_NAMES = {joint_names_[0]}; -// const std::vector IF_NAMES = {custom_interface_name_}; -// SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); - -// // configure ok -// ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// const size_t NUM_JOINTS = JOINT_NAMES.size(); - -// // check interface configuration -// auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); -// ASSERT_THAT(cmd_if_conf.names, IsEmpty()); -// EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); -// auto state_if_conf = state_broadcaster_->state_interface_configuration(); -// ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); -// EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - -// // joint state initialized -// const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; -// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); -// ASSERT_THAT(joint_state_msg.name, SizeIs(0)); -// ASSERT_THAT(joint_state_msg.position, SizeIs(0)); -// ASSERT_THAT(joint_state_msg.velocity, SizeIs(0)); -// ASSERT_THAT(joint_state_msg.effort, SizeIs(0)); - -// // dynamic joint state initialized -// const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; -// ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); -// ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); -// ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); -// ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); -// ASSERT_THAT( -// dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray(IF_NAMES)); - -// // publishers initialized -// ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); -// ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); -// } - -// TEST_F(GenericStateBroadcasterTest, TestCustomInterfaceMapping) -// { -// const std::vector JOINT_NAMES = {joint_names_[0]}; -// const std::vector IF_NAMES = {custom_interface_name_}; -// SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); - -// state_broadcaster_->get_node()->set_parameter( -// {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); - -// // configure ok -// ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// const size_t NUM_JOINTS = JOINT_NAMES.size(); - -// // check interface configuration -// auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); -// ASSERT_THAT(cmd_if_conf.names, IsEmpty()); -// EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); -// auto state_if_conf = state_broadcaster_->state_interface_configuration(); -// ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); -// EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - -// // joint state initialized -// const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; -// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); -// ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); -// ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); -// ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); -// for (auto i = 0ul; i < NUM_JOINTS; ++i) -// { -// ASSERT_TRUE(std::isnan(joint_state_msg.velocity[i])); -// } -// ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); -// for (auto i = 0ul; i < NUM_JOINTS; ++i) -// { -// ASSERT_TRUE(std::isnan(joint_state_msg.effort[i])); -// } - -// // dynamic joint state initialized -// const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; -// ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); -// ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); -// ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); -// ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); -// ASSERT_THAT( -// dynamic_joint_state_msg.interface_values[0].interface_names, -// ElementsAreArray({HW_IF_POSITION})); // mapping to this value - -// // publishers initialized -// ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); -// ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); -// } - -// TEST_F(GenericStateBroadcasterTest, TestCustomInterfaceMappingUpdate) -// { -// const std::vector JOINT_NAMES = {joint_names_[0]}; -// const std::vector IF_NAMES = {custom_interface_name_}; -// SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES); - -// state_broadcaster_->get_node()->set_parameter( -// {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_}); - -// sensor_msgs::msg::JointState joint_state_msg; -// activate_and_get_joint_state_message("joint_states", joint_state_msg); - -// const size_t NUM_JOINTS = JOINT_NAMES.size(); - -// // joint state initialized -// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); -// ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); -// ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); -// ASSERT_EQ(joint_state_msg.position[0], custom_joint_value_); -// ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); -// for (auto i = 0ul; i < NUM_JOINTS; ++i) -// { -// ASSERT_TRUE(std::isnan(joint_state_msg.velocity[i])); -// } -// ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); -// for (auto i = 0ul; i < NUM_JOINTS; ++i) -// { -// ASSERT_TRUE(std::isnan(joint_state_msg.effort[i])); -// } - -// // dynamic joint state initialized -// const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; -// ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); -// ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); -// ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); -// ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); -// ASSERT_THAT( -// dynamic_joint_state_msg.interface_values[0].interface_names, -// ElementsAreArray({HW_IF_POSITION})); - -// // publishers initialized -// ASSERT_TRUE(state_broadcaster_->joint_state_publisher_); -// ASSERT_TRUE(state_broadcaster_->dynamic_joint_state_publisher_); -// } - -// TEST_F(GenericStateBroadcasterTest, UpdateTest) -// { -// SetUpStateBroadcaster(); - -// auto node_state = state_broadcaster_->configure(); -// node_state = state_broadcaster_->get_node()->activate(); -// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); -// ASSERT_EQ( -// state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); -// } + ASSERT_EQ( + state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); -// TEST_F(GenericStateBroadcasterTest, UpdatePerformanceTest) -// { -// controller_interface::ControllerInterfaceParams params; -// params.controller_name = "joint_state_broadcaster"; -// params.robot_description = ""; -// params.update_rate = 0; -// params.node_namespace = ""; -// params.node_options = state_broadcaster_->define_custom_node_options(); -// const auto result = state_broadcaster_->init(params); -// ASSERT_EQ(result, controller_interface::return_type::OK); - -// custom_joint_value_ = 12.34; - -// // build our own test interfaces: robot has ~500 state interfaces -// for (auto joint = 1u; joint < 30; ++joint) -// { -// const auto joint_name = "joint_" + std::to_string(joint); - -// // standard -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "position", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "velocity", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "effort", &custom_joint_value_)); - -// // non standard -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "mode", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "absolute_position", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "acceleration", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "current", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "torque", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "force", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "temperature_board", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "temperature_motor", &custom_joint_value_)); - -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "position.kd", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "position.ki", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "position.kp", &custom_joint_value_)); - -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "velocity.kd", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "velocity.ki", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "velocity.kp", &custom_joint_value_)); - -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "current.kd", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "current.ki", &custom_joint_value_)); -// test_interfaces_.emplace_back( -// std::make_shared( -// joint_name, "current.kp", &custom_joint_value_)); -// } - -// RCLCPP_INFO( -// state_broadcaster_->get_node()->get_logger(), "Number of test interfaces: %zu", -// test_interfaces_.size()); - -// std::vector state_interfaces; -// for (const auto & tif : test_interfaces_) -// { -// state_interfaces.emplace_back(tif, nullptr); -// } - -// state_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); - -// auto node_state = state_broadcaster_->configure(); -// node_state = state_broadcaster_->get_node()->activate(); -// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - -// if (!realtime_tools::configure_sched_fifo(50)) -// { -// RCLCPP_WARN( -// state_broadcaster_->get_node()->get_logger(), -// "Could not enable FIFO RT scheduling policy: with error number <%i>(%s)", errno, -// strerror(errno)); -// } - -// constexpr auto kNumSamples = 10000u; -// std::vector measures; -// for (auto i = 0u; i < kNumSamples; ++i) -// { -// const auto now = std::chrono::steady_clock::now(); - -// ASSERT_EQ( -// state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), -// controller_interface::return_type::OK); - -// // print time taken -// const auto elapsed = std::chrono::steady_clock::now() - now; -// const auto elapsed_us = std::chrono::duration_cast(elapsed); - -// measures.push_back(elapsed_us.count()); -// } - -// const auto average = -// std::accumulate(measures.begin(), measures.end(), 0.0) / static_cast(measures.size()); -// const auto variance = std::accumulate( -// measures.begin(), measures.end(), 0.0, [average](double accum, double x) -// { return accum + (x - average) * (x - average); }) / -// static_cast(measures.size()); - -// RCLCPP_INFO(state_broadcaster_->get_node()->get_logger(), "Average update time: %lf us", average); -// RCLCPP_INFO(state_broadcaster_->get_node()->get_logger(), "Variance: %lf us", variance); -// } - -// void GenericStateBroadcasterTest::activate_and_get_joint_state_message( -// const std::string & topic, sensor_msgs::msg::JointState & joint_state_msg) -// { -// auto node_state = state_broadcaster_->configure(); -// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - -// node_state = state_broadcaster_->get_node()->activate(); -// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - -// sensor_msgs::msg::JointState::SharedPtr received_msg; -// rclcpp::Node test_node("test_node"); -// auto subs_callback = [&](const sensor_msgs::msg::JointState::SharedPtr msg) -// { received_msg = msg; }; -// auto subscription = -// test_node.create_subscription(topic, 10, subs_callback); - -// rclcpp::executors::SingleThreadedExecutor executor; -// executor.add_node(test_node.get_node_base_interface()); - -// // call update to publish the test value -// // since update doesn't guarantee a published message, republish until received -// int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop -// while (max_sub_check_loop_count--) -// { -// state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// const auto timeout = std::chrono::milliseconds{5}; -// const auto until = test_node.get_clock()->now() + timeout; -// while (!received_msg && test_node.get_clock()->now() < until) -// { -// executor.spin_some(); -// std::this_thread::sleep_for(std::chrono::microseconds(10)); -// } -// // check if message has been received -// if (received_msg.get()) -// { -// break; -// } -// } -// ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " -// "controller/broadcaster update loop"; -// ASSERT_TRUE(received_msg); - -// // take message from subscription -// joint_state_msg = *received_msg; -// } - -// void GenericStateBroadcasterTest::test_published_joint_state_message(const std::string & topic) -// { -// sensor_msgs::msg::JointState joint_state_msg; -// activate_and_get_joint_state_message(topic, joint_state_msg); - -// const size_t NUM_JOINTS = joint_names_.size(); -// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); -// ASSERT_THAT(joint_state_msg.name, SizeIs(NUM_JOINTS)); -// // the order in the message may be different -// // we only check that all values in this test are present in the message -// // and that it is the same across the interfaces -// // for test purposes they are mapped to the same doubles -// ASSERT_THAT(joint_state_msg.position, ElementsAreArray(joint_values_)); -// ASSERT_THAT(joint_state_msg.velocity, ElementsAreArray(joint_state_msg.position)); -// ASSERT_THAT(joint_state_msg.effort, ElementsAreArray(joint_state_msg.position)); -// } - -// TEST_F(GenericStateBroadcasterTest, JointStatePublishTest) -// { -// SetUpStateBroadcaster(); - -// test_published_joint_state_message("joint_states"); -// } - -// TEST_F(GenericStateBroadcasterTest, JointStatePublishTestLocalTopic) -// { -// SetUpStateBroadcaster(); -// state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); - -// test_published_joint_state_message("joint_state_broadcaster/joint_states"); -// } - -// void GenericStateBroadcasterTest::test_published_dynamic_joint_state_message( -// const std::string & topic) -// { -// auto node_state = state_broadcaster_->configure(); -// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - -// node_state = state_broadcaster_->get_node()->activate(); -// ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - -// control_msgs::msg::DynamicJointState::SharedPtr dynamic_joint_state_msg; -// rclcpp::Node test_node("test_node"); -// auto subs_callback = [&](const control_msgs::msg::DynamicJointState::SharedPtr msg) -// { dynamic_joint_state_msg = msg; }; -// auto subscription = -// test_node.create_subscription(topic, 10, subs_callback); -// rclcpp::executors::SingleThreadedExecutor executor; -// executor.add_node(test_node.get_node_base_interface()); -// dynamic_joint_state_msg.reset(); -// ASSERT_FALSE(dynamic_joint_state_msg); - -// // call update to publish the test value -// // since update doesn't guarantee a published message, republish until received -// int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop -// while (max_sub_check_loop_count--) -// { -// state_broadcaster_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); -// const auto timeout = std::chrono::milliseconds{5}; -// const auto until = test_node.get_clock()->now() + timeout; -// while (test_node.get_clock()->now() < until) -// { -// executor.spin_some(); -// std::this_thread::sleep_for(std::chrono::microseconds(10)); -// } -// // check if message has been received -// if (dynamic_joint_state_msg.get()) -// { -// break; -// } -// } -// ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " -// "controller/broadcaster update loop"; -// ASSERT_TRUE(dynamic_joint_state_msg); - -// const size_t NUM_JOINTS = 3; -// const std::vector INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; -// ASSERT_EQ(dynamic_joint_state_msg->header.frame_id, frame_id_); -// ASSERT_THAT(dynamic_joint_state_msg->joint_names, SizeIs(NUM_JOINTS)); -// // the order in the message may be different -// // we only check that all values in this test are present in the message -// // and that it is the same across the interfaces -// // for test purposes they are mapped to the same doubles -// for (size_t i = 0; i < dynamic_joint_state_msg->joint_names.size(); ++i) -// { -// ASSERT_THAT( -// dynamic_joint_state_msg->interface_values[i].interface_names, -// ElementsAreArray(INTERFACE_NAMES)); -// const auto index_in_original_order = static_cast(std::distance( -// joint_names_.cbegin(), -// std::find( -// joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i]))); -// ASSERT_THAT( -// dynamic_joint_state_msg->interface_values[i].values, -// Each(joint_values_[index_in_original_order])); -// } -// } - -// TEST_F(GenericStateBroadcasterTest, DynamicJointStatePublishTest) -// { -// SetUpStateBroadcaster(); - -// test_published_dynamic_joint_state_message("dynamic_joint_states"); -// } - -// TEST_F(GenericStateBroadcasterTest, DynamicJointStatePublishTestLocalTopic) -// { -// SetUpStateBroadcaster(); -// state_broadcaster_->get_node()->set_parameter({"use_local_topics", true}); - -// test_published_dynamic_joint_state_message("joint_state_broadcaster/dynamic_joint_states"); -// } - -// TEST_F(GenericStateBroadcasterTest, ExtraJointStatePublishTest) -// { -// // publishers not initialized yet -// ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_); -// ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); - -// SetUpStateBroadcaster(); - -// // Add extra joints as parameters -// const std::vector extra_joint_names = {"extra1", "extra2", "extra3"}; -// state_broadcaster_->get_node()->set_parameter({"extra_joints", extra_joint_names}); - -// // configure ok -// ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - -// ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(state_broadcaster_->values_msg_.values.size(), 3); + ASSERT_DOUBLE_EQ(state_broadcaster_->values_msg_.values[0], joint_values_[0]); + ASSERT_DOUBLE_EQ(state_broadcaster_->values_msg_.values[1], joint_values_[1]); + ASSERT_DOUBLE_EQ(state_broadcaster_->values_msg_.values[2], joint_values_[2]); + ASSERT_THAT(state_broadcaster_->names_msg_.names, ElementsAreArray(interfaces)); +} -// std::vector all_joint_names = joint_names_; -// all_joint_names.insert(all_joint_names.end(), extra_joint_names.begin(), extra_joint_names.end()); -// const size_t NUM_JOINTS = all_joint_names.size(); +TEST_F(GenericStateBroadcasterTest, StatePublishTest) +{ + std::vector all_interfaces; + for (const auto & joint_name : joint_names_) + { + for (const auto & interface_name : interface_names_) + { + all_interfaces.push_back(joint_name + "/" + interface_name); + } + } + SetUpStateBroadcaster(all_interfaces); -// // joint state initialized -// const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; -// ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); -// ASSERT_THAT(joint_state_msg.name, ElementsAreArray(all_joint_names)); -// ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); -// ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); -// ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + control_msgs::msg::InterfacesValues values_msg; + activate_and_get_state_message("generic_state_broadcaster/values", values_msg); -// // dynamic joint state initialized -// const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; -// ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); -// ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); -// } + ASSERT_THAT(values_msg.values, SizeIs(9)); + // all values are NaN since we did not set any value to the state interfaces + const std::vector expected_values = { + joint_values_[0], joint_values_[0], joint_values_[0], joint_values_[1], joint_values_[1], + joint_values_[1], joint_values_[2], joint_values_[2], joint_values_[2], + }; + ASSERT_THAT(values_msg.values, ElementsAreArray(expected_values)); +} diff --git a/generic_state_broadcaster/test/test_generic_state_broadcaster.hpp b/generic_state_broadcaster/test/test_generic_state_broadcaster.hpp index 748e5a0e82..d92db20c22 100644 --- a/generic_state_broadcaster/test/test_generic_state_broadcaster.hpp +++ b/generic_state_broadcaster/test/test_generic_state_broadcaster.hpp @@ -34,21 +34,7 @@ class FriendGenericStateBroadcaster : public generic_state_broadcaster::GenericS { FRIEND_TEST(GenericStateBroadcasterTest, FailOnEmptyInterfaceListTest); FRIEND_TEST(GenericStateBroadcasterTest, ConfigureOnValidInterfaceListTest); - FRIEND_TEST(GenericStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithoutJointsParameter); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithJointsAndInterfaces); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing); - FRIEND_TEST(GenericStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing); - FRIEND_TEST(GenericStateBroadcasterTest, TestCustomInterfaceWithoutMapping); - FRIEND_TEST(GenericStateBroadcasterTest, TestCustomInterfaceMapping); - FRIEND_TEST(GenericStateBroadcasterTest, TestCustomInterfaceMappingUpdate); - FRIEND_TEST(GenericStateBroadcasterTest, ExtraJointStatePublishTest); + FRIEND_TEST(GenericStateBroadcasterTest, StatePublishTest); }; class GenericStateBroadcasterTest : public ::testing::Test @@ -73,7 +59,7 @@ class GenericStateBroadcasterTest : public ::testing::Test void test_published_dynamic_joint_state_message(const std::string & topic); void activate_and_get_state_message( - const std::string & topic, control_msgs::msg::ValuesArray & msg); + const std::string & topic, control_msgs::msg::InterfacesValues & msg); protected: // dummy joint state values used for tests From 6e2980f54276ec930f6a0e59b89c8c66c2492bcc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Nov 2025 21:46:36 +0100 Subject: [PATCH 5/7] add state broadcaster params file --- generic_state_broadcaster/CMakeLists.txt | 3 +++ .../test/test_generic_state_broadcaster_params.yaml | 3 +++ .../test/test_load_generic_state_broadcaster.cpp | 3 +++ 3 files changed, 9 insertions(+) create mode 100644 generic_state_broadcaster/test/test_generic_state_broadcaster_params.yaml diff --git a/generic_state_broadcaster/CMakeLists.txt b/generic_state_broadcaster/CMakeLists.txt index c2c5709841..4c43470cc1 100644 --- a/generic_state_broadcaster/CMakeLists.txt +++ b/generic_state_broadcaster/CMakeLists.txt @@ -60,6 +60,9 @@ if(BUILD_TESTING) hardware_interface::hardware_interface rclcpp::rclcpp ros2_control_test_assets::ros2_control_test_assets) + target_compile_definitions( + test_load_generic_state_broadcaster + PRIVATE PARAMETERS_FILE_PATH="${CMAKE_CURRENT_LIST_DIR}/test/test_generic_state_broadcaster_params.yaml") ament_add_gmock(test_generic_state_broadcaster test/test_generic_state_broadcaster.cpp diff --git a/generic_state_broadcaster/test/test_generic_state_broadcaster_params.yaml b/generic_state_broadcaster/test/test_generic_state_broadcaster_params.yaml new file mode 100644 index 0000000000..4b40508512 --- /dev/null +++ b/generic_state_broadcaster/test/test_generic_state_broadcaster_params.yaml @@ -0,0 +1,3 @@ +test_generic_state_broadcaster: + ros__parameters: + interfaces: ["joint1/position", "joint2/velocity"] \ No newline at end of file diff --git a/generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp b/generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp index 8b3725cd71..75d4da7425 100644 --- a/generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp +++ b/generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp @@ -32,6 +32,9 @@ TEST(TestLoadGenericStateBroadcaster, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + cm.set_parameter( + rclcpp::Parameter( + "test_generic_state_broadcaster.params_file", std::string(PARAMETERS_FILE_PATH))); ASSERT_NE( cm.load_controller( "test_generic_state_broadcaster", "generic_state_broadcaster/GenericStateBroadcaster"), From bfe9c8072eae59581358bd03ddfc532e3cef9486 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Nov 2025 21:57:33 +0100 Subject: [PATCH 6/7] rename the package to interfaces state broadcaster --- .../generic_state_broadcaster_plugin.xml | 7 --- ...test_generic_state_broadcaster_params.yaml | 3 -- .../CMakeLists.txt | 52 +++++++++---------- .../interfaces_state_broadcaster.hpp | 24 ++++----- .../interfaces_state_broadcaster_plugin.xml | 7 +++ .../package.xml | 4 +- .../src/interfaces_state_broadcaster.cpp | 26 +++++----- ...terfaces_state_broadcaster_parameters.yaml | 4 +- .../test_interfaces_state_broadcaster.cpp | 36 +++++++------ .../test_interfaces_state_broadcaster.hpp | 21 ++++---- ...t_interfaces_state_broadcaster_params.yaml | 3 ++ ...test_load_interfaces_state_broadcaster.cpp | 7 +-- 12 files changed, 100 insertions(+), 94 deletions(-) delete mode 100644 generic_state_broadcaster/generic_state_broadcaster_plugin.xml delete mode 100644 generic_state_broadcaster/test/test_generic_state_broadcaster_params.yaml rename {generic_state_broadcaster => interfaces_state_broadcaster}/CMakeLists.txt (54%) rename generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp => interfaces_state_broadcaster/include/interfaces_state_broadcaster/interfaces_state_broadcaster.hpp (73%) create mode 100644 interfaces_state_broadcaster/interfaces_state_broadcaster_plugin.xml rename {generic_state_broadcaster => interfaces_state_broadcaster}/package.xml (94%) rename generic_state_broadcaster/src/generic_state_broadcaster.cpp => interfaces_state_broadcaster/src/interfaces_state_broadcaster.cpp (80%) rename generic_state_broadcaster/src/generic_state_broadcaster_parameters.yaml => interfaces_state_broadcaster/src/interfaces_state_broadcaster_parameters.yaml (70%) rename generic_state_broadcaster/test/test_generic_state_broadcaster.cpp => interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.cpp (88%) rename generic_state_broadcaster/test/test_generic_state_broadcaster.hpp => interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.hpp (85%) create mode 100644 interfaces_state_broadcaster/test/test_interfaces_state_broadcaster_params.yaml rename generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp => interfaces_state_broadcaster/test/test_load_interfaces_state_broadcaster.cpp (83%) diff --git a/generic_state_broadcaster/generic_state_broadcaster_plugin.xml b/generic_state_broadcaster/generic_state_broadcaster_plugin.xml deleted file mode 100644 index ac9bdf1b94..0000000000 --- a/generic_state_broadcaster/generic_state_broadcaster_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The generic state broadcaster publishes the values of the requested interfaces from ros2_control system. - - - diff --git a/generic_state_broadcaster/test/test_generic_state_broadcaster_params.yaml b/generic_state_broadcaster/test/test_generic_state_broadcaster_params.yaml deleted file mode 100644 index 4b40508512..0000000000 --- a/generic_state_broadcaster/test/test_generic_state_broadcaster_params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -test_generic_state_broadcaster: - ros__parameters: - interfaces: ["joint1/position", "joint2/velocity"] \ No newline at end of file diff --git a/generic_state_broadcaster/CMakeLists.txt b/interfaces_state_broadcaster/CMakeLists.txt similarity index 54% rename from generic_state_broadcaster/CMakeLists.txt rename to interfaces_state_broadcaster/CMakeLists.txt index 4c43470cc1..fe9837726d 100644 --- a/generic_state_broadcaster/CMakeLists.txt +++ b/interfaces_state_broadcaster/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(generic_state_broadcaster) +project(interfaces_state_broadcaster) find_package(ros2_control_cmake REQUIRED) set_compiler_options() @@ -21,20 +21,20 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -generate_parameter_library(generic_state_broadcaster_parameters - src/generic_state_broadcaster_parameters.yaml +generate_parameter_library(interfaces_state_broadcaster_parameters + src/interfaces_state_broadcaster_parameters.yaml ) -add_library(generic_state_broadcaster SHARED - src/generic_state_broadcaster.cpp +add_library(interfaces_state_broadcaster SHARED + src/interfaces_state_broadcaster.cpp ) -target_compile_features(generic_state_broadcaster PUBLIC cxx_std_17) -target_include_directories(generic_state_broadcaster PUBLIC +target_compile_features(interfaces_state_broadcaster PUBLIC cxx_std_17) +target_include_directories(interfaces_state_broadcaster PUBLIC $ - $ + $ ) -target_link_libraries(generic_state_broadcaster PUBLIC - generic_state_broadcaster_parameters +target_link_libraries(interfaces_state_broadcaster PUBLIC + interfaces_state_broadcaster_parameters controller_interface::controller_interface pluginlib::pluginlib rclcpp::rclcpp @@ -42,7 +42,7 @@ target_link_libraries(generic_state_broadcaster PUBLIC realtime_tools::realtime_tools ${control_msgs_TARGETS} ${builtin_interfaces_TARGETS}) -pluginlib_export_plugin_description_file(controller_interface generic_state_broadcaster_plugin.xml) +pluginlib_export_plugin_description_file(controller_interface interfaces_state_broadcaster_plugin.xml) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -51,41 +51,41 @@ if(BUILD_TESTING) find_package(rclcpp REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_generic_state_broadcaster - test/test_load_generic_state_broadcaster.cpp + ament_add_gmock(test_load_interfaces_state_broadcaster + test/test_load_interfaces_state_broadcaster.cpp ) - target_link_libraries(test_load_generic_state_broadcaster - generic_state_broadcaster + target_link_libraries(test_load_interfaces_state_broadcaster + interfaces_state_broadcaster controller_manager::controller_manager hardware_interface::hardware_interface rclcpp::rclcpp ros2_control_test_assets::ros2_control_test_assets) target_compile_definitions( - test_load_generic_state_broadcaster - PRIVATE PARAMETERS_FILE_PATH="${CMAKE_CURRENT_LIST_DIR}/test/test_generic_state_broadcaster_params.yaml") + test_load_interfaces_state_broadcaster + PRIVATE PARAMETERS_FILE_PATH="${CMAKE_CURRENT_LIST_DIR}/test/test_interfaces_state_broadcaster_params.yaml") - ament_add_gmock(test_generic_state_broadcaster - test/test_generic_state_broadcaster.cpp + ament_add_gmock(test_interfaces_state_broadcaster + test/test_interfaces_state_broadcaster.cpp ) - target_link_libraries(test_generic_state_broadcaster - generic_state_broadcaster + target_link_libraries(test_interfaces_state_broadcaster + interfaces_state_broadcaster ros2_control_test_assets::ros2_control_test_assets) endif() install( DIRECTORY include/ - DESTINATION include/generic_state_broadcaster + DESTINATION include/interfaces_state_broadcaster ) install( TARGETS - generic_state_broadcaster - generic_state_broadcaster_parameters - EXPORT export_generic_state_broadcaster + interfaces_state_broadcaster + interfaces_state_broadcaster_parameters + EXPORT export_interfaces_state_broadcaster RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib ) -ament_export_targets(export_generic_state_broadcaster HAS_LIBRARY_TARGET) +ament_export_targets(export_interfaces_state_broadcaster HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp b/interfaces_state_broadcaster/include/interfaces_state_broadcaster/interfaces_state_broadcaster.hpp similarity index 73% rename from generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp rename to interfaces_state_broadcaster/include/interfaces_state_broadcaster/interfaces_state_broadcaster.hpp index 5c9f579cb2..9a99b31f24 100644 --- a/generic_state_broadcaster/include/generic_state_broadcaster/generic_state_broadcaster.hpp +++ b/interfaces_state_broadcaster/include/interfaces_state_broadcaster/interfaces_state_broadcaster.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GENERIC_STATE_BROADCASTER__GENERIC_STATE_BROADCASTER_HPP_ -#define GENERIC_STATE_BROADCASTER__GENERIC_STATE_BROADCASTER_HPP_ +#ifndef INTERFACES_STATE_BROADCASTER__INTERFACES_STATE_BROADCASTER_HPP_ +#define INTERFACES_STATE_BROADCASTER__INTERFACES_STATE_BROADCASTER_HPP_ #include #include @@ -27,29 +27,29 @@ #include "realtime_tools/realtime_publisher.hpp" // auto-generated by generate_parameter_library -#include "generic_state_broadcaster/generic_state_broadcaster_parameters.hpp" +#include "interfaces_state_broadcaster/interfaces_state_broadcaster_parameters.hpp" -namespace generic_state_broadcaster +namespace interfaces_state_broadcaster { /** - * \brief Generic State Broadcaster for selected state interfaces in a ros2_control system. + * \brief Interfaces State Broadcaster for selected state interfaces in a ros2_control system. * - * GenericStateBroadcaster publishes the selected state interfaces from ros2_control as ROS + * InterfacesStateBroadcaster publishes the selected state interfaces from ros2_control as ROS * messages. * * Publishes to: * - \b ~/names (control_msgs::msg::StringArray): The list of the interface names that are selected - * to be published by the generic state broadcaster. This is published with transient local + * to be published by the interfaces state broadcaster. This is published with transient local * durability. * - \b ~/values (control_msgs::msg::ValuesArray): The list of the values corresponding to the - * interface names that are selected to be published by the generic state broadcaster. + * interface names that are selected to be published by the interfaces state broadcaster. * * \note The values are published at the same rate as the controller update rate. */ -class GenericStateBroadcaster : public controller_interface::ControllerInterface +class InterfacesStateBroadcaster : public controller_interface::ControllerInterface { public: - GenericStateBroadcaster(); + InterfacesStateBroadcaster(); controller_interface::InterfaceConfiguration command_interface_configuration() const override; @@ -80,6 +80,6 @@ class GenericStateBroadcaster : public controller_interface::ControllerInterface control_msgs::msg::InterfacesNames names_msg_; }; -} // namespace generic_state_broadcaster +} // namespace interfaces_state_broadcaster -#endif // GENERIC_STATE_BROADCASTER__GENERIC_STATE_BROADCASTER_HPP_ +#endif // INTERFACES_STATE_BROADCASTER__INTERFACES_STATE_BROADCASTER_HPP_ diff --git a/interfaces_state_broadcaster/interfaces_state_broadcaster_plugin.xml b/interfaces_state_broadcaster/interfaces_state_broadcaster_plugin.xml new file mode 100644 index 0000000000..fe6671f494 --- /dev/null +++ b/interfaces_state_broadcaster/interfaces_state_broadcaster_plugin.xml @@ -0,0 +1,7 @@ + + + + The interfaces state broadcaster publishes the values of the requested interfaces from ros2_control system. + + + diff --git a/generic_state_broadcaster/package.xml b/interfaces_state_broadcaster/package.xml similarity index 94% rename from generic_state_broadcaster/package.xml rename to interfaces_state_broadcaster/package.xml index c997ed9c79..0bc5b4cc3b 100644 --- a/generic_state_broadcaster/package.xml +++ b/interfaces_state_broadcaster/package.xml @@ -1,8 +1,8 @@ - generic_state_broadcaster + interfaces_state_broadcaster 5.8.0 - Broadcaster to publish desired hardware interface states + Broadcaster to publish desired hardware interface states that are castable to double Bence Magyar Denis Štogl diff --git a/generic_state_broadcaster/src/generic_state_broadcaster.cpp b/interfaces_state_broadcaster/src/interfaces_state_broadcaster.cpp similarity index 80% rename from generic_state_broadcaster/src/generic_state_broadcaster.cpp rename to interfaces_state_broadcaster/src/interfaces_state_broadcaster.cpp index a897a81ca1..7dccb88762 100644 --- a/generic_state_broadcaster/src/generic_state_broadcaster.cpp +++ b/interfaces_state_broadcaster/src/interfaces_state_broadcaster.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "generic_state_broadcaster/generic_state_broadcaster.hpp" +#include "interfaces_state_broadcaster/interfaces_state_broadcaster.hpp" #include #include @@ -25,12 +25,12 @@ #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" -namespace generic_state_broadcaster +namespace interfaces_state_broadcaster { -GenericStateBroadcaster::GenericStateBroadcaster() {} +InterfacesStateBroadcaster::InterfacesStateBroadcaster() {} -controller_interface::CallbackReturn GenericStateBroadcaster::on_init() +controller_interface::CallbackReturn InterfacesStateBroadcaster::on_init() { try { @@ -47,14 +47,14 @@ controller_interface::CallbackReturn GenericStateBroadcaster::on_init() } controller_interface::InterfaceConfiguration -GenericStateBroadcaster::command_interface_configuration() const +InterfacesStateBroadcaster::command_interface_configuration() const { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } controller_interface::InterfaceConfiguration -GenericStateBroadcaster::state_interface_configuration() const +InterfacesStateBroadcaster::state_interface_configuration() const { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -65,7 +65,7 @@ GenericStateBroadcaster::state_interface_configuration() const return state_interfaces_config; } -controller_interface::CallbackReturn GenericStateBroadcaster::on_configure( +controller_interface::CallbackReturn InterfacesStateBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); @@ -88,7 +88,7 @@ controller_interface::CallbackReturn GenericStateBroadcaster::on_configure( return CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn GenericStateBroadcaster::on_activate( +controller_interface::CallbackReturn InterfacesStateBroadcaster::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { for (auto i = 0u; i < state_interfaces_.size(); ++i) @@ -97,7 +97,8 @@ controller_interface::CallbackReturn GenericStateBroadcaster::on_activate( { RCLCPP_ERROR( get_node()->get_logger(), - "State interface '%s' is not castable to double. The GenericStateBroadcaster only supports " + "State interface '%s' is not castable to double. The InterfacesStateBroadcaster only " + "supports " "state interfaces that can be casted to double.", params_.interfaces[i].c_str()); return CallbackReturn::FAILURE; @@ -107,7 +108,7 @@ controller_interface::CallbackReturn GenericStateBroadcaster::on_activate( return CallbackReturn::SUCCESS; } -controller_interface::return_type GenericStateBroadcaster::update( +controller_interface::return_type InterfacesStateBroadcaster::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { for (auto i = 0u; i < state_interfaces_.size(); ++i) @@ -128,9 +129,10 @@ controller_interface::return_type GenericStateBroadcaster::update( return controller_interface::return_type::OK; } -} // namespace generic_state_broadcaster +} // namespace interfaces_state_broadcaster #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - generic_state_broadcaster::GenericStateBroadcaster, controller_interface::ControllerInterface) + interfaces_state_broadcaster::InterfacesStateBroadcaster, + controller_interface::ControllerInterface) diff --git a/generic_state_broadcaster/src/generic_state_broadcaster_parameters.yaml b/interfaces_state_broadcaster/src/interfaces_state_broadcaster_parameters.yaml similarity index 70% rename from generic_state_broadcaster/src/generic_state_broadcaster_parameters.yaml rename to interfaces_state_broadcaster/src/interfaces_state_broadcaster_parameters.yaml index 7f23e806d9..405b5a31a0 100644 --- a/generic_state_broadcaster/src/generic_state_broadcaster_parameters.yaml +++ b/interfaces_state_broadcaster/src/interfaces_state_broadcaster_parameters.yaml @@ -1,8 +1,8 @@ -generic_state_broadcaster: +interfaces_state_broadcaster: interfaces: { type: string_array, default_value: [], - description: "The list of hardware interfaces information to be published by the generic state broadcaster.", + description: "The list of hardware interfaces information to be published by the interfaces state broadcaster.", read_only: true, validation: { not_empty<>: [] diff --git a/generic_state_broadcaster/test/test_generic_state_broadcaster.cpp b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.cpp similarity index 88% rename from generic_state_broadcaster/test/test_generic_state_broadcaster.cpp rename to interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.cpp index 86f098729e..9a1d2ed25a 100644 --- a/generic_state_broadcaster/test/test_generic_state_broadcaster.cpp +++ b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.cpp @@ -29,7 +29,7 @@ #include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" -#include "test_generic_state_broadcaster.hpp" +#include "test_interfaces_state_broadcaster.hpp" using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; @@ -47,24 +47,25 @@ constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; } // namespace -void GenericStateBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +void InterfacesStateBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } -void GenericStateBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } +void InterfacesStateBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } -void GenericStateBroadcasterTest::SetUp() +void InterfacesStateBroadcasterTest::SetUp() { // initialize broadcaster - state_broadcaster_ = std::make_unique(); + state_broadcaster_ = std::make_unique(); } -void GenericStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); } +void InterfacesStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); } -controller_interface::return_type GenericStateBroadcasterTest::SetUpStateBroadcaster( +controller_interface::return_type InterfacesStateBroadcasterTest::SetUpStateBroadcaster( const std::vector & interfaces) { RCLCPP_INFO( - rclcpp::get_logger("GenericStateBroadcasterTest"), - "Setting up GenericStateBroadcaster with interfaces: %d", static_cast(interfaces.size())); + rclcpp::get_logger("InterfacesStateBroadcasterTest"), + "Setting up InterfacesStateBroadcaster with interfaces: %d", + static_cast(interfaces.size())); auto result = init_broadcaster_and_set_parameters("", interfaces); if (result == controller_interface::return_type::OK) { @@ -73,11 +74,12 @@ controller_interface::return_type GenericStateBroadcasterTest::SetUpStateBroadca return result; } -controller_interface::return_type GenericStateBroadcasterTest::init_broadcaster_and_set_parameters( +controller_interface::return_type +InterfacesStateBroadcasterTest::init_broadcaster_and_set_parameters( const std::string & robot_description, const std::vector & interfaces) { controller_interface::ControllerInterfaceParams params; - params.controller_name = "generic_state_broadcaster"; + params.controller_name = "interfaces_state_broadcaster"; params.robot_description = robot_description; params.update_rate = 0; params.node_namespace = ""; @@ -90,7 +92,7 @@ controller_interface::return_type GenericStateBroadcasterTest::init_broadcaster_ return state_broadcaster_->init(params); } -void GenericStateBroadcasterTest::assign_state_interfaces( +void InterfacesStateBroadcasterTest::assign_state_interfaces( const std::vector & interfaces) { std::vector state_ifs; @@ -158,7 +160,7 @@ void GenericStateBroadcasterTest::assign_state_interfaces( state_broadcaster_->assign_interfaces({}, std::move(state_ifs)); } -void GenericStateBroadcasterTest::activate_and_get_state_message( +void InterfacesStateBroadcasterTest::activate_and_get_state_message( const std::string & topic, control_msgs::msg::InterfacesValues & msg) { auto node_state = state_broadcaster_->configure(); @@ -204,7 +206,7 @@ void GenericStateBroadcasterTest::activate_and_get_state_message( msg = *received_msg; } -TEST_F(GenericStateBroadcasterTest, FailOnEmptyInterfaceListTest) +TEST_F(InterfacesStateBroadcasterTest, FailOnEmptyInterfaceListTest) { // publishers not initialized yet ASSERT_FALSE(state_broadcaster_->names_publisher_); @@ -213,7 +215,7 @@ TEST_F(GenericStateBroadcasterTest, FailOnEmptyInterfaceListTest) ASSERT_EQ(SetUpStateBroadcaster({}), controller_interface::return_type::ERROR); } -TEST_F(GenericStateBroadcasterTest, ConfigureOnValidInterfaceListTest) +TEST_F(InterfacesStateBroadcasterTest, ConfigureOnValidInterfaceListTest) { // publishers not initialized yet ASSERT_FALSE(state_broadcaster_->names_publisher_); @@ -254,7 +256,7 @@ TEST_F(GenericStateBroadcasterTest, ConfigureOnValidInterfaceListTest) ASSERT_THAT(state_broadcaster_->names_msg_.names, ElementsAreArray(interfaces)); } -TEST_F(GenericStateBroadcasterTest, StatePublishTest) +TEST_F(InterfacesStateBroadcasterTest, StatePublishTest) { std::vector all_interfaces; for (const auto & joint_name : joint_names_) @@ -267,7 +269,7 @@ TEST_F(GenericStateBroadcasterTest, StatePublishTest) SetUpStateBroadcaster(all_interfaces); control_msgs::msg::InterfacesValues values_msg; - activate_and_get_state_message("generic_state_broadcaster/values", values_msg); + activate_and_get_state_message("interfaces_state_broadcaster/values", values_msg); ASSERT_THAT(values_msg.values, SizeIs(9)); // all values are NaN since we did not set any value to the state interfaces diff --git a/generic_state_broadcaster/test/test_generic_state_broadcaster.hpp b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.hpp similarity index 85% rename from generic_state_broadcaster/test/test_generic_state_broadcaster.hpp rename to interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.hpp index d92db20c22..97852b73a4 100644 --- a/generic_state_broadcaster/test/test_generic_state_broadcaster.hpp +++ b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEST_GENERIC_STATE_BROADCASTER_HPP_ -#define TEST_GENERIC_STATE_BROADCASTER_HPP_ +#ifndef TEST_INTERFACES_STATE_BROADCASTER_HPP_ +#define TEST_INTERFACES_STATE_BROADCASTER_HPP_ #include @@ -21,7 +21,7 @@ #include #include -#include "generic_state_broadcaster/generic_state_broadcaster.hpp" +#include "interfaces_state_broadcaster/interfaces_state_broadcaster.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -30,14 +30,15 @@ using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; // subclassing and friending so we can access member variables -class FriendGenericStateBroadcaster : public generic_state_broadcaster::GenericStateBroadcaster +class FriendInterfacesStateBroadcaster +: public interfaces_state_broadcaster::InterfacesStateBroadcaster { - FRIEND_TEST(GenericStateBroadcasterTest, FailOnEmptyInterfaceListTest); - FRIEND_TEST(GenericStateBroadcasterTest, ConfigureOnValidInterfaceListTest); - FRIEND_TEST(GenericStateBroadcasterTest, StatePublishTest); + FRIEND_TEST(InterfacesStateBroadcasterTest, FailOnEmptyInterfaceListTest); + FRIEND_TEST(InterfacesStateBroadcasterTest, ConfigureOnValidInterfaceListTest); + FRIEND_TEST(InterfacesStateBroadcasterTest, StatePublishTest); }; -class GenericStateBroadcasterTest : public ::testing::Test +class InterfacesStateBroadcasterTest : public ::testing::Test { public: static void SetUpTestCase(); @@ -103,8 +104,8 @@ class GenericStateBroadcasterTest : public ::testing::Test std::vector test_interfaces_; - std::unique_ptr state_broadcaster_; + std::unique_ptr state_broadcaster_; std::string frame_id_ = "base_link"; }; -#endif // TEST_GENERIC_STATE_BROADCASTER_HPP_ +#endif // TEST_INTERFACES_STATE_BROADCASTER_HPP_ diff --git a/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster_params.yaml b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster_params.yaml new file mode 100644 index 0000000000..4f3b3a0e35 --- /dev/null +++ b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster_params.yaml @@ -0,0 +1,3 @@ +test_interfaces_state_broadcaster: + ros__parameters: + interfaces: ["joint1/position", "joint2/velocity"] diff --git a/generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp b/interfaces_state_broadcaster/test/test_load_interfaces_state_broadcaster.cpp similarity index 83% rename from generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp rename to interfaces_state_broadcaster/test/test_load_interfaces_state_broadcaster.cpp index 75d4da7425..0e1615f69f 100644 --- a/generic_state_broadcaster/test/test_load_generic_state_broadcaster.cpp +++ b/interfaces_state_broadcaster/test/test_load_interfaces_state_broadcaster.cpp @@ -22,7 +22,7 @@ #include "rclcpp/utilities.hpp" #include "ros2_control_test_assets/descriptions.hpp" -TEST(TestLoadGenericStateBroadcaster, load_controller) +TEST(TestLoadInterfacesStateBroadcaster, load_controller) { rclcpp::init(0, nullptr); @@ -34,10 +34,11 @@ TEST(TestLoadGenericStateBroadcaster, load_controller) cm.set_parameter( rclcpp::Parameter( - "test_generic_state_broadcaster.params_file", std::string(PARAMETERS_FILE_PATH))); + "test_interfaces_state_broadcaster.params_file", std::string(PARAMETERS_FILE_PATH))); ASSERT_NE( cm.load_controller( - "test_generic_state_broadcaster", "generic_state_broadcaster/GenericStateBroadcaster"), + "test_interfaces_state_broadcaster", + "interfaces_state_broadcaster/InterfacesStateBroadcaster"), nullptr); rclcpp::shutdown(); From a0b0b1cde980ab823b6273d1443b13b26ed66a3e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Nov 2025 22:01:46 +0100 Subject: [PATCH 7/7] Add release_notes --- doc/release_notes.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 8622620620..6572730866 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -4,3 +4,7 @@ Release Notes: Kilted Kaiju to Lyrical Luth ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes important changes between Kilted Kaiju (previous) and Lyrical Luth (current) releases. + +interfaces_state_broadcaster +********************************* +* 🚀 The interfaces_state_broadcaster was added 🎉 (`#2006 `_).