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 `_). diff --git a/interfaces_state_broadcaster/CMakeLists.txt b/interfaces_state_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..fe9837726d --- /dev/null +++ b/interfaces_state_broadcaster/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.16) +project(interfaces_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(interfaces_state_broadcaster_parameters + src/interfaces_state_broadcaster_parameters.yaml +) + +add_library(interfaces_state_broadcaster SHARED + src/interfaces_state_broadcaster.cpp +) +target_compile_features(interfaces_state_broadcaster PUBLIC cxx_std_17) +target_include_directories(interfaces_state_broadcaster PUBLIC + $ + $ +) +target_link_libraries(interfaces_state_broadcaster PUBLIC + interfaces_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 interfaces_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_interfaces_state_broadcaster + test/test_load_interfaces_state_broadcaster.cpp + ) + 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_interfaces_state_broadcaster + PRIVATE PARAMETERS_FILE_PATH="${CMAKE_CURRENT_LIST_DIR}/test/test_interfaces_state_broadcaster_params.yaml") + + ament_add_gmock(test_interfaces_state_broadcaster + test/test_interfaces_state_broadcaster.cpp + ) + target_link_libraries(test_interfaces_state_broadcaster + interfaces_state_broadcaster + ros2_control_test_assets::ros2_control_test_assets) +endif() + +install( + DIRECTORY include/ + DESTINATION include/interfaces_state_broadcaster +) +install( + TARGETS + 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_interfaces_state_broadcaster HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/interfaces_state_broadcaster/include/interfaces_state_broadcaster/interfaces_state_broadcaster.hpp b/interfaces_state_broadcaster/include/interfaces_state_broadcaster/interfaces_state_broadcaster.hpp new file mode 100644 index 0000000000..9a99b31f24 --- /dev/null +++ b/interfaces_state_broadcaster/include/interfaces_state_broadcaster/interfaces_state_broadcaster.hpp @@ -0,0 +1,85 @@ +// 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 INTERFACES_STATE_BROADCASTER__INTERFACES_STATE_BROADCASTER_HPP_ +#define INTERFACES_STATE_BROADCASTER__INTERFACES_STATE_BROADCASTER_HPP_ + +#include +#include +#include +#include + +#include "control_msgs/msg/dynamic_joint_state.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" + +// auto-generated by generate_parameter_library +#include "interfaces_state_broadcaster/interfaces_state_broadcaster_parameters.hpp" + +namespace interfaces_state_broadcaster +{ +/** + * \brief Interfaces State Broadcaster for selected state interfaces in a ros2_control system. + * + * 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 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 interfaces state broadcaster. + * + * \note The values are published at the same rate as the controller update rate. + */ +class InterfacesStateBroadcaster : public controller_interface::ControllerInterface +{ +public: + InterfacesStateBroadcaster(); + + 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::InterfacesValues values_msg_; + control_msgs::msg::InterfacesNames names_msg_; +}; + +} // namespace interfaces_state_broadcaster + +#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/interfaces_state_broadcaster/package.xml b/interfaces_state_broadcaster/package.xml new file mode 100644 index 0000000000..0bc5b4cc3b --- /dev/null +++ b/interfaces_state_broadcaster/package.xml @@ -0,0 +1,43 @@ + + + interfaces_state_broadcaster + 5.8.0 + Broadcaster to publish desired hardware interface states that are castable to double + + 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/interfaces_state_broadcaster/src/interfaces_state_broadcaster.cpp b/interfaces_state_broadcaster/src/interfaces_state_broadcaster.cpp new file mode 100644 index 0000000000..7dccb88762 --- /dev/null +++ b/interfaces_state_broadcaster/src/interfaces_state_broadcaster.cpp @@ -0,0 +1,138 @@ +// 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 "interfaces_state_broadcaster/interfaces_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 interfaces_state_broadcaster +{ + +InterfacesStateBroadcaster::InterfacesStateBroadcaster() {} + +controller_interface::CallbackReturn InterfacesStateBroadcaster::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 params_.interfaces.empty() ? CallbackReturn::ERROR : CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +InterfacesStateBroadcaster::command_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration +InterfacesStateBroadcaster::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 InterfacesStateBroadcaster::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()); + + names_msg_.names = params_.interfaces; + names_msg_.header.stamp = get_node()->now(); + names_publisher_->publish(names_msg_); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn InterfacesStateBroadcaster::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 InterfacesStateBroadcaster 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 InterfacesStateBroadcaster::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 interfaces_state_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + interfaces_state_broadcaster::InterfacesStateBroadcaster, + controller_interface::ControllerInterface) diff --git a/interfaces_state_broadcaster/src/interfaces_state_broadcaster_parameters.yaml b/interfaces_state_broadcaster/src/interfaces_state_broadcaster_parameters.yaml new file mode 100644 index 0000000000..405b5a31a0 --- /dev/null +++ b/interfaces_state_broadcaster/src/interfaces_state_broadcaster_parameters.yaml @@ -0,0 +1,10 @@ +interfaces_state_broadcaster: + interfaces: { + type: string_array, + default_value: [], + description: "The list of hardware interfaces information to be published by the interfaces state broadcaster.", + read_only: true, + validation: { + not_empty<>: [] + } + } diff --git a/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.cpp b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.cpp new file mode 100644 index 0000000000..9a1d2ed25a --- /dev/null +++ b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.cpp @@ -0,0 +1,281 @@ +// 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_interfaces_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 InterfacesStateBroadcasterTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void InterfacesStateBroadcasterTest::TearDownTestCase() { rclcpp::shutdown(); } + +void InterfacesStateBroadcasterTest::SetUp() +{ + // initialize broadcaster + state_broadcaster_ = std::make_unique(); +} + +void InterfacesStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); } + +controller_interface::return_type InterfacesStateBroadcasterTest::SetUpStateBroadcaster( + const std::vector & interfaces) +{ + RCLCPP_INFO( + 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) + { + assign_state_interfaces(interfaces); + } + return result; +} + +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 = "interfaces_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 InterfacesStateBroadcasterTest::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)); +} + +void InterfacesStateBroadcasterTest::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(InterfacesStateBroadcasterTest, 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(InterfacesStateBroadcasterTest, 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_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); + + 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)); +} + +TEST_F(InterfacesStateBroadcasterTest, 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); + + control_msgs::msg::InterfacesValues 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 + 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/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.hpp b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.hpp new file mode 100644 index 0000000000..97852b73a4 --- /dev/null +++ b/interfaces_state_broadcaster/test/test_interfaces_state_broadcaster.hpp @@ -0,0 +1,111 @@ +// 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_INTERFACES_STATE_BROADCASTER_HPP_ +#define TEST_INTERFACES_STATE_BROADCASTER_HPP_ + +#include + +#include +#include +#include + +#include "interfaces_state_broadcaster/interfaces_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 FriendInterfacesStateBroadcaster +: public interfaces_state_broadcaster::InterfacesStateBroadcaster +{ + FRIEND_TEST(InterfacesStateBroadcasterTest, FailOnEmptyInterfaceListTest); + FRIEND_TEST(InterfacesStateBroadcasterTest, ConfigureOnValidInterfaceListTest); + FRIEND_TEST(InterfacesStateBroadcasterTest, StatePublishTest); +}; + +class InterfacesStateBroadcasterTest : 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::InterfacesValues & 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_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/interfaces_state_broadcaster/test/test_load_interfaces_state_broadcaster.cpp b/interfaces_state_broadcaster/test/test_load_interfaces_state_broadcaster.cpp new file mode 100644 index 0000000000..0e1615f69f --- /dev/null +++ b/interfaces_state_broadcaster/test/test_load_interfaces_state_broadcaster.cpp @@ -0,0 +1,45 @@ +// 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(TestLoadInterfacesStateBroadcaster, 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"); + + cm.set_parameter( + rclcpp::Parameter( + "test_interfaces_state_broadcaster.params_file", std::string(PARAMETERS_FILE_PATH))); + ASSERT_NE( + cm.load_controller( + "test_interfaces_state_broadcaster", + "interfaces_state_broadcaster/InterfacesStateBroadcaster"), + nullptr); + + rclcpp::shutdown(); +}