From 29cfc45e81d51186f501e976810667dfdac43935 Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 5 Nov 2020 12:09:11 -0800 Subject: [PATCH] Add rclcpp_action action_server benchmarks (#1433) * Add rclcpp_action action_server benchmarks Signed-off-by: Stephen Brawner * Address cancel bug Signed-off-by: Stephen Brawner * Fix errors Signed-off-by: Stephen Brawner * Fix clang error Signed-off-by: Stephen Brawner --- rclcpp_action/test/benchmark/CMakeLists.txt | 9 + .../benchmark/benchmark_action_client.cpp | 3 + .../benchmark/benchmark_action_server.cpp | 318 ++++++++++++++++++ 3 files changed, 330 insertions(+) create mode 100644 rclcpp_action/test/benchmark/benchmark_action_server.cpp diff --git a/rclcpp_action/test/benchmark/CMakeLists.txt b/rclcpp_action/test/benchmark/CMakeLists.txt index 9a3b8d0..b9aabf2 100644 --- a/rclcpp_action/test/benchmark/CMakeLists.txt +++ b/rclcpp_action/test/benchmark/CMakeLists.txt @@ -12,3 +12,12 @@ if(TARGET benchmark_action_client) target_link_libraries(benchmark_action_client ${PROJECT_NAME}) ament_target_dependencies(benchmark_action_client rclcpp test_msgs) endif() + +add_performance_test( + benchmark_action_server + benchmark_action_server.cpp + TIMEOUT 120) +if(TARGET benchmark_action_server) + target_link_libraries(benchmark_action_server ${PROJECT_NAME}) + ament_target_dependencies(benchmark_action_server rclcpp test_msgs) +endif() diff --git a/rclcpp_action/test/benchmark/benchmark_action_client.cpp b/rclcpp_action/test/benchmark/benchmark_action_client.cpp index c703a98..8d2482f 100644 --- a/rclcpp_action/test/benchmark/benchmark_action_client.cpp +++ b/rclcpp_action/test/benchmark/benchmark_action_client.cpp @@ -112,6 +112,9 @@ public: protected: std::shared_ptr node; std::shared_ptr> action_server; + + // Goal handle needs to be kept alive by the server in order for client request specific to the + // goal to succeed. std::shared_ptr current_goal_handle; }; diff --git a/rclcpp_action/test/benchmark/benchmark_action_server.cpp b/rclcpp_action/test/benchmark/benchmark_action_server.cpp new file mode 100644 index 0000000..2259a68 --- /dev/null +++ b/rclcpp_action/test/benchmark/benchmark_action_server.cpp @@ -0,0 +1,318 @@ +// 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_action/rclcpp_action.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/action/fibonacci.hpp" + +using performance_test_fixture::PerformanceTest; + +using Fibonacci = test_msgs::action::Fibonacci; +using GoalHandle = rclcpp_action::ServerGoalHandle; +using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response; +using GoalUUID = rclcpp_action::GoalUUID; + +constexpr char fibonacci_action_name[] = "fibonacci"; + +class ActionServerPerformanceTest : public PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + action_client = + rclcpp_action::create_client(node, fibonacci_action_name); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + + action_client.reset(); + node.reset(); + rclcpp::shutdown(); + } + + auto AsyncSendGoalOfOrder(const int order) + { + test_msgs::action::Fibonacci::Goal goal; + goal.order = order; + + return action_client->async_send_goal(goal); + } + +protected: + std::shared_ptr node; + std::shared_ptr> action_client; +}; + +BENCHMARK_F(ActionServerPerformanceTest, construct_server_without_client)(benchmark::State & state) +{ + constexpr char action_name[] = "no_corresponding_client"; + for (auto _ : state) { + auto action_server = rclcpp_action::create_server( + node, action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [](std::shared_ptr) {}); + benchmark::DoNotOptimize(action_server); + benchmark::ClobberMemory(); + + state.PauseTiming(); + action_server.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, construct_server_with_client)(benchmark::State & state) +{ + for (auto _ : state) { + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [](std::shared_ptr) {}); + benchmark::DoNotOptimize(action_server); + benchmark::ClobberMemory(); + + state.PauseTiming(); + action_server.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, destroy_server)(benchmark::State & state) +{ + for (auto _ : state) { + state.PauseTiming(); + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [](std::shared_ptr) {}); + state.ResumeTiming(); + benchmark::DoNotOptimize(action_server); + benchmark::ClobberMemory(); + + action_server.reset(); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_accept_goal)(benchmark::State & state) +{ + std::shared_ptr current_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [¤t_goal_handle](std::shared_ptr goal_handle) { + current_goal_handle = goal_handle; + }); + + reset_heap_counters(); + for (auto _ : state) { + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(1); + state.ResumeTiming(); + + rclcpp::spin_until_future_complete(node, client_goal_handle_future); + auto goal_handle = client_goal_handle_future.get(); + if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { + state.SkipWithError("Valid goal was not accepted"); + return; + } + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_cancel_goal)(benchmark::State & state) +{ + // The goal handle needs to be assigned to a variable for the lifetime of the goal so that it is + // not cleaned up before the cancel request is received and processed. + std::shared_ptr server_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [&server_goal_handle](std::shared_ptr goal_handle) { + server_goal_handle = goal_handle; + }); + + reset_heap_counters(); + for (auto _ : state) { + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(1); + // This spin completes when the goal has been accepted, but not executed because server + // responds with ACCEPT_AND_DEFER + rclcpp::spin_until_future_complete(node, client_goal_handle_future, std::chrono::seconds(1)); + auto client_goal_handle = client_goal_handle_future.get(); + auto future_cancel = action_client->async_cancel_goal(client_goal_handle); + state.ResumeTiming(); + + rclcpp::spin_until_future_complete(node, future_cancel, std::chrono::seconds(1)); + auto cancel_response = future_cancel.get(); + using CancelResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; + if (CancelResponse::ERROR_NONE != cancel_response->return_code) { + state.SkipWithError("Cancel request did not succeed"); + break; + } + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_execute_goal)(benchmark::State & state) +{ + std::shared_ptr server_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [&server_goal_handle](std::shared_ptr goal_handle) { + server_goal_handle = goal_handle; + }); + + reset_heap_counters(); + for (auto _ : state) { + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(1); + + rclcpp::spin_until_future_complete(node, client_goal_handle_future); + auto goal_handle = client_goal_handle_future.get(); + if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { + state.SkipWithError("Valid goal was not accepted"); + return; + } + state.ResumeTiming(); + + server_goal_handle->execute(); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_set_success)(benchmark::State & state) +{ + constexpr int goal_order = 1; + std::shared_ptr server_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [&server_goal_handle](std::shared_ptr goal_handle) { + server_goal_handle = goal_handle; + }); + + // MSVC and Clang disagree how goal_order should be captured here. Though this capture is a bit + // too wide, they at least could agree it was fine. In my testing MSVC errored if goal_order was + // not captured, but clang would warn if it was explicitly captured. + const auto result = [&]() { + auto result = std::make_shared(); + for (int i = 0; i < goal_order; ++i) { + // Not the fibonacci sequence, but that's not important to this benchmark + result->sequence.push_back(i); + } + return result; + } (); + + reset_heap_counters(); + for (auto _ : state) { + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); + + rclcpp::spin_until_future_complete(node, client_goal_handle_future); + auto goal_handle = client_goal_handle_future.get(); + if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { + state.SkipWithError("Valid goal was not accepted"); + return; + } + server_goal_handle->execute(); + state.ResumeTiming(); + + server_goal_handle->succeed(result); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_abort)(benchmark::State & state) +{ + constexpr int goal_order = 1; + std::shared_ptr server_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [&server_goal_handle](std::shared_ptr goal_handle) { + server_goal_handle = goal_handle; + }); + + // Capturing with & because MSVC and Clang disagree about how to capture goal_order + const auto result = [&]() { + auto result = std::make_shared(); + for (int i = 0; i < goal_order; ++i) { + // Not the fibonacci sequence, but that's not important to this benchmark + result->sequence.push_back(i); + } + return result; + } (); + + reset_heap_counters(); + for (auto _ : state) { + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); + + rclcpp::spin_until_future_complete(node, client_goal_handle_future); + auto goal_handle = client_goal_handle_future.get(); + if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { + state.SkipWithError("Valid goal was not accepted"); + return; + } + server_goal_handle->execute(); + state.ResumeTiming(); + + server_goal_handle->abort(result); + } +}