From 041748d17f331bf1c52cc43ae19509364f8aa550 Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Tue, 20 Oct 2020 16:59:44 -0700 Subject: [PATCH 1/3] Add service and client benchmarks Signed-off-by: Stephen Brawner --- rclcpp/test/benchmark/CMakeLists.txt | 12 ++ rclcpp/test/benchmark/benchmark_client.cpp | 153 ++++++++++++++++++++ rclcpp/test/benchmark/benchmark_service.cpp | 140 ++++++++++++++++++ 3 files changed, 305 insertions(+) create mode 100644 rclcpp/test/benchmark/benchmark_client.cpp create mode 100644 rclcpp/test/benchmark/benchmark_service.cpp diff --git a/rclcpp/test/benchmark/CMakeLists.txt b/rclcpp/test/benchmark/CMakeLists.txt index d7d7e53956..f680f29a34 100644 --- a/rclcpp/test/benchmark/CMakeLists.txt +++ b/rclcpp/test/benchmark/CMakeLists.txt @@ -4,6 +4,12 @@ find_package(performance_test_fixture REQUIRED) # implementation. We are looking to test the performance of the ROS 2 code, not # the underlying middleware. +add_performance_test(benchmark_client benchmark_client.cpp) +if(TARGET benchmark_client) + target_link_libraries(benchmark_client ${PROJECT_NAME}) + ament_target_dependencies(benchmark_client test_msgs rcl_interfaces) +endif() + add_performance_test(benchmark_init_shutdown benchmark_init_shutdown.cpp) if(TARGET benchmark_init_shutdown) target_link_libraries(benchmark_init_shutdown ${PROJECT_NAME}) @@ -13,3 +19,9 @@ add_performance_test(benchmark_node benchmark_node.cpp) if(TARGET benchmark_node) target_link_libraries(benchmark_node ${PROJECT_NAME}) endif() + +add_performance_test(benchmark_service benchmark_service.cpp) +if(TARGET benchmark_service) + target_link_libraries(benchmark_service ${PROJECT_NAME}) + ament_target_dependencies(benchmark_service test_msgs rcl_interfaces) +endif() diff --git a/rclcpp/test/benchmark/benchmark_client.cpp b/rclcpp/test/benchmark/benchmark_client.cpp new file mode 100644 index 0000000000..4c1064632a --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_client.cpp @@ -0,0 +1,153 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/srv/empty.hpp" + +using performance_test_fixture::PerformanceTest; + +constexpr char empty_service_name[] = "empty_service"; + +class ClientPerformanceTest : public PerformanceTest +{ +public: + explicit ClientPerformanceTest(rclcpp::NodeOptions = rclcpp::NodeOptions()) {} + void SetUp(::benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_unique("node", "ns"); + + auto empty_service_callback = + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + empty_service = + node->create_service(empty_service_name, empty_service_callback); + + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(::benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + empty_service.reset(); + node.reset(); + rclcpp::shutdown(); + } + +protected: + std::unique_ptr node; + std::shared_ptr> empty_service; +}; + +BENCHMARK_F(ClientPerformanceTest, construct_client_no_service)(benchmark::State & state) { + // Prime cache + auto outer_client = node->create_client("not_an_existing_service"); + outer_client.reset(); + + reset_heap_counters(); + for (auto _ : state) { + auto client = node->create_client("not_an_existing_service"); + benchmark::DoNotOptimize(client); + benchmark::ClobberMemory(); + + state.PauseTiming(); + client.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ClientPerformanceTest, construct_client_empty_srv)(benchmark::State & state) { + // Prime cache + auto outer_client = node->create_client(empty_service_name); + outer_client.reset(); + + reset_heap_counters(); + for (auto _ : state) { + auto client = node->create_client(empty_service_name); + benchmark::DoNotOptimize(client); + benchmark::ClobberMemory(); + + state.PauseTiming(); + client.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & state) { + // Prime cache + auto outer_client = node->create_client(empty_service_name); + outer_client.reset(); + + reset_heap_counters(); + for (auto _ : state) { + state.PauseTiming(); + auto client = node->create_client(empty_service_name); + state.ResumeTiming(); + benchmark::DoNotOptimize(client); + benchmark::ClobberMemory(); + + client.reset(); + } +} + +BENCHMARK_F(ClientPerformanceTest, wait_for_service)(benchmark::State & state) { + int count = 0; + for (auto _ : state) { + state.PauseTiming(); + const std::string service_name = std::string("service_") + std::to_string(count++); + // Create client before service so it has to 'discover' the service after construction + auto client = node->create_client(service_name); + auto callback = + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node->create_service(service_name, callback); + state.ResumeTiming(); + + client->wait_for_service(std::chrono::seconds(1)); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(ClientPerformanceTest, async_send_request_only)(benchmark::State & state) { + auto client = node->create_client(empty_service_name); + auto shared_request = std::make_shared(); + + reset_heap_counters(); + for (auto _ : state) { + auto future = client->async_send_request(shared_request); + benchmark::DoNotOptimize(future); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::State & state) { + auto client = node->create_client(empty_service_name); + auto shared_request = std::make_shared(); + + reset_heap_counters(); + for (auto _ : state) { + auto future = client->async_send_request(shared_request); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), future, std::chrono::seconds(1)); + benchmark::DoNotOptimize(future); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp/test/benchmark/benchmark_service.cpp b/rclcpp/test/benchmark/benchmark_service.cpp new file mode 100644 index 0000000000..8aed491a6e --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_service.cpp @@ -0,0 +1,140 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/srv/empty.hpp" + +using performance_test_fixture::PerformanceTest; + +constexpr char empty_service_name[] = "empty_service"; + +class ServicePerformanceTest : public PerformanceTest +{ +public: + ServicePerformanceTest() : callback_count(0) {} + + void SetUp(::benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_unique("node", "ns"); + empty_client = node->create_client(empty_service_name); + callback_count = 0; + + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(::benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + + empty_client.reset(); + node.reset(); + rclcpp::shutdown(); + } + + void ServiceCallback( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) + { + callback_count++; + } + +protected: + std::unique_ptr node; + std::shared_ptr> empty_client; + int callback_count; +}; + +BENCHMARK_F(ServicePerformanceTest, construct_service_no_client)(benchmark::State & state) { + auto callback = std::bind( + &ServicePerformanceTest::ServiceCallback, + this, std::placeholders::_1, std::placeholders::_2); + + auto outer_service = node->create_service("not_a_service", callback); + + reset_heap_counters(); + for (auto _ : state) { + auto service = node->create_service("not_a_service", callback); + benchmark::DoNotOptimize(service); + benchmark::ClobberMemory(); + + state.PauseTiming(); + service.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ServicePerformanceTest, construct_service_empty_srv)(benchmark::State & state) { + auto callback = std::bind( + &ServicePerformanceTest::ServiceCallback, + this, std::placeholders::_1, std::placeholders::_2); + auto outer_service = node->create_service(empty_service_name, callback); + + reset_heap_counters(); + for (auto _ : state) { + auto service = node->create_service(empty_service_name, callback); + benchmark::DoNotOptimize(service); + benchmark::ClobberMemory(); + + state.PauseTiming(); + service.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ServicePerformanceTest, destroy_service_empty_srv)(benchmark::State & state) { + auto callback = std::bind( + &ServicePerformanceTest::ServiceCallback, + this, std::placeholders::_1, std::placeholders::_2); + auto outer_service = node->create_service(empty_service_name, callback); + + reset_heap_counters(); + for (auto _ : state) { + state.PauseTiming(); + auto service = node->create_service(empty_service_name, callback); + state.ResumeTiming(); + benchmark::DoNotOptimize(service); + benchmark::ClobberMemory(); + + service.reset(); + } +} + +BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & state) { + auto callback = std::bind( + &ServicePerformanceTest::ServiceCallback, + this, std::placeholders::_1, std::placeholders::_2); + auto service = node->create_service(empty_service_name, callback); + + reset_heap_counters(); + for (auto _ : state) { + state.PauseTiming(); + // Clear executor queue + rclcpp::spin_some(node->get_node_base_interface()); + + auto request = std::make_shared(); + auto future = empty_client->async_send_request(request); + state.ResumeTiming(); + benchmark::DoNotOptimize(service); + benchmark::ClobberMemory(); + + rclcpp::spin_until_future_complete(node->get_node_base_interface(), future); + } + if (callback_count == 0) { + state.SkipWithError("Service callback was not called"); + } +} From 4c1d89c3247de90a5e12f588b025c2187bccde2c Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Mon, 26 Oct 2020 10:42:45 -0700 Subject: [PATCH 2/3] Style Signed-off-by: Stephen Brawner --- rclcpp/test/benchmark/benchmark_client.cpp | 4 ++-- rclcpp/test/benchmark/benchmark_service.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/test/benchmark/benchmark_client.cpp b/rclcpp/test/benchmark/benchmark_client.cpp index 4c1064632a..05608623a0 100644 --- a/rclcpp/test/benchmark/benchmark_client.cpp +++ b/rclcpp/test/benchmark/benchmark_client.cpp @@ -27,7 +27,7 @@ class ClientPerformanceTest : public PerformanceTest { public: explicit ClientPerformanceTest(rclcpp::NodeOptions = rclcpp::NodeOptions()) {} - void SetUp(::benchmark::State & state) + void SetUp(benchmark::State & state) { rclcpp::init(0, nullptr); node = std::make_unique("node", "ns"); @@ -42,7 +42,7 @@ class ClientPerformanceTest : public PerformanceTest performance_test_fixture::PerformanceTest::SetUp(state); } - void TearDown(::benchmark::State & state) + void TearDown(benchmark::State & state) { performance_test_fixture::PerformanceTest::TearDown(state); empty_service.reset(); diff --git a/rclcpp/test/benchmark/benchmark_service.cpp b/rclcpp/test/benchmark/benchmark_service.cpp index 8aed491a6e..e138f9b2ca 100644 --- a/rclcpp/test/benchmark/benchmark_service.cpp +++ b/rclcpp/test/benchmark/benchmark_service.cpp @@ -27,17 +27,17 @@ class ServicePerformanceTest : public PerformanceTest public: ServicePerformanceTest() : callback_count(0) {} - void SetUp(::benchmark::State & state) + void SetUp(benchmark::State & state) { rclcpp::init(0, nullptr); node = std::make_unique("node", "ns"); empty_client = node->create_client(empty_service_name); callback_count = 0; - + performance_test_fixture::PerformanceTest::SetUp(state); } - void TearDown(::benchmark::State & state) + void TearDown(benchmark::State & state) { performance_test_fixture::PerformanceTest::TearDown(state); From 58bd67386d03af28e1320d4df8431469390ba1bd Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Mon, 26 Oct 2020 10:57:10 -0700 Subject: [PATCH 3/3] Uncrustify Signed-off-by: Stephen Brawner --- rclcpp/test/benchmark/benchmark_service.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/benchmark/benchmark_service.cpp b/rclcpp/test/benchmark/benchmark_service.cpp index e138f9b2ca..f1d9aca68b 100644 --- a/rclcpp/test/benchmark/benchmark_service.cpp +++ b/rclcpp/test/benchmark/benchmark_service.cpp @@ -25,7 +25,8 @@ constexpr char empty_service_name[] = "empty_service"; class ServicePerformanceTest : public PerformanceTest { public: - ServicePerformanceTest() : callback_count(0) {} + ServicePerformanceTest() + : callback_count(0) {} void SetUp(benchmark::State & state) {