diff --git a/rclcpp/test/benchmark/CMakeLists.txt b/rclcpp/test/benchmark/CMakeLists.txt index bec3e47..07a5aac 100644 --- a/rclcpp/test/benchmark/CMakeLists.txt +++ b/rclcpp/test/benchmark/CMakeLists.txt @@ -26,6 +26,11 @@ if(TARGET benchmark_node) target_link_libraries(benchmark_node ${PROJECT_NAME}) endif() +add_performance_test(benchmark_node_parameters_interface benchmark_node_parameters_interface.cpp) +if(TARGET benchmark_node_parameters_interface) + target_link_libraries(benchmark_node_parameters_interface ${PROJECT_NAME}) +endif() + add_performance_test(benchmark_service benchmark_service.cpp) if(TARGET benchmark_service) target_link_libraries(benchmark_service ${PROJECT_NAME}) diff --git a/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp b/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp new file mode 100644 index 0000000..26c0bcb --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp @@ -0,0 +1,262 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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 "performance_test_fixture/performance_test_fixture.hpp" + +#include "rclcpp/rclcpp.hpp" + +class NodeParametersInterfaceTest : public performance_test_fixture::PerformanceTest +{ +public: + NodeParametersInterfaceTest() + : node_name("my_node"), + param_prefix("my_prefix"), + param1_name(param_prefix + ".my_param_1"), + param2_name(param_prefix + ".my_param_2"), + param3_name(param_prefix + ".my_param_3") + { + } + + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_shared(node_name); + + node->declare_parameter(param1_name); + node->declare_parameter(param2_name); + node->declare_parameter(param3_name); + node->undeclare_parameter(param3_name); + + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + + node.reset(); + rclcpp::shutdown(); + } + + const std::string node_name; + const std::string param_prefix; + const std::string param1_name; + const std::string param2_name; + const std::string param3_name; + +protected: + rclcpp::Node::SharedPtr node; +}; + +BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & state) +{ + for (auto _ : state) { + node->declare_parameter(param3_name); + node->undeclare_parameter(param3_name); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & state) +{ + for (auto _ : state) { + if (!node->has_parameter(param1_name)) { + state.SkipWithError("Parameter was expected"); + break; + } + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_miss)(benchmark::State & state) +{ + for (auto _ : state) { + if (node->has_parameter(param3_name)) { + state.SkipWithError("Parameter was not expected"); + break; + } + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_bool)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, true), + rclcpp::Parameter(param2_name, false), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, false), + rclcpp::Parameter(param2_name, true), + }; + + reset_heap_counters(); + + for (auto _ : state) { + node->set_parameters(param_values2); + node->set_parameters(param_values1); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_atomically_bool)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, true), + rclcpp::Parameter(param2_name, false), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, false), + rclcpp::Parameter(param2_name, true), + }; + + reset_heap_counters(); + + for (auto _ : state) { + node->set_parameters_atomically(param_values2); + node->set_parameters_atomically(param_values1); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_callback_bool)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, true), + rclcpp::Parameter(param2_name, false), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, false), + rclcpp::Parameter(param2_name, true), + }; + + rcl_interfaces::msg::SetParametersResult callback_result; + bool callback_received = false; + callback_result.successful = true; + auto callback = + [&callback_result, &callback_received](const std::vector &) { + callback_received = true; + return callback_result; + }; + auto handle = node->add_on_set_parameters_callback(callback); + + reset_heap_counters(); + + for (auto _ : state) { + node->set_parameters(param_values2); + node->set_parameters(param_values1); + } + + if (!callback_received) { + state.SkipWithError("Callback is not functioning"); + } + + node->remove_on_set_parameters_callback(handle.get()); +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_string)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, "param 1 value A"), + rclcpp::Parameter(param2_name, "param 2 value B"), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, "param 1 value B"), + rclcpp::Parameter(param2_name, "param 2 value A"), + }; + + reset_heap_counters(); + + for (auto _ : state) { + node->set_parameters(param_values2); + node->set_parameters(param_values1); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_array)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, std::vector {0, 1, 2}), + rclcpp::Parameter(param2_name, std::vector {3, 4, 5}), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, std::vector {4, 5, 6}), + rclcpp::Parameter(param2_name, std::vector {7, 8, 9}), + }; + + reset_heap_counters(); + + for (auto _ : state) { + node->set_parameters(param_values2); + node->set_parameters(param_values1); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, get_parameter)(benchmark::State & state) +{ + rclcpp::Parameter param1_value; + + reset_heap_counters(); + + for (auto _ : state) { + node->get_parameter(param1_name, param1_value); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_hit)(benchmark::State & state) +{ + rcl_interfaces::msg::ListParametersResult param_list; + const std::vector prefixes + { + param_prefix, + }; + + reset_heap_counters(); + + for (auto _ : state) { + param_list = node->list_parameters(prefixes, 10); + if (param_list.names.size() != 2) { + state.SkipWithError("Expected node names"); + break; + } + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_miss)(benchmark::State & state) +{ + rcl_interfaces::msg::ListParametersResult param_list; + const std::vector prefixes + { + "your_param", + }; + + reset_heap_counters(); + + for (auto _ : state) { + param_list = node->list_parameters(prefixes, 10); + if (param_list.names.size() != 0) { + state.SkipWithError("Expected no node names"); + break; + } + } +}