Add rclcpp_action action_server benchmarks (#1433)
* Add rclcpp_action action_server benchmarks Signed-off-by: Stephen Brawner <brawner@gmail.com> * Address cancel bug Signed-off-by: Stephen Brawner <brawner@gmail.com> * Fix errors Signed-off-by: Stephen Brawner <brawner@gmail.com> * Fix clang error Signed-off-by: Stephen Brawner <brawner@gmail.com>
This commit is contained in:
parent
5607c3242d
commit
29cfc45e81
3 changed files with 330 additions and 0 deletions
|
@ -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()
|
||||
|
|
|
@ -112,6 +112,9 @@ public:
|
|||
protected:
|
||||
std::shared_ptr<rclcpp::Node> node;
|
||||
std::shared_ptr<rclcpp_action::Server<test_msgs::action::Fibonacci>> 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<GoalHandle> current_goal_handle;
|
||||
};
|
||||
|
||||
|
|
318
rclcpp_action/test/benchmark/benchmark_action_server.cpp
Normal file
318
rclcpp_action/test/benchmark/benchmark_action_server.cpp
Normal file
|
@ -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 <memory>
|
||||
#include <string>
|
||||
|
||||
#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<Fibonacci>;
|
||||
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<rclcpp::Node>("node", "ns");
|
||||
action_client =
|
||||
rclcpp_action::create_client<Fibonacci>(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<rclcpp::Node> node;
|
||||
std::shared_ptr<rclcpp_action::Client<Fibonacci>> 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<Fibonacci>(
|
||||
node, action_name,
|
||||
[](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {});
|
||||
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<Fibonacci>(
|
||||
node, fibonacci_action_name,
|
||||
[](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {});
|
||||
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<Fibonacci>(
|
||||
node, fibonacci_action_name,
|
||||
[](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {});
|
||||
state.ResumeTiming();
|
||||
benchmark::DoNotOptimize(action_server);
|
||||
benchmark::ClobberMemory();
|
||||
|
||||
action_server.reset();
|
||||
}
|
||||
}
|
||||
|
||||
BENCHMARK_F(ActionServerPerformanceTest, action_server_accept_goal)(benchmark::State & state)
|
||||
{
|
||||
std::shared_ptr<GoalHandle> current_goal_handle = nullptr;
|
||||
auto action_server = rclcpp_action::create_server<Fibonacci>(
|
||||
node, fibonacci_action_name,
|
||||
[](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
},
|
||||
[¤t_goal_handle](std::shared_ptr<GoalHandle> 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<GoalHandle> server_goal_handle = nullptr;
|
||||
auto action_server = rclcpp_action::create_server<Fibonacci>(
|
||||
node, fibonacci_action_name,
|
||||
[](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
},
|
||||
[&server_goal_handle](std::shared_ptr<GoalHandle> 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<GoalHandle> server_goal_handle = nullptr;
|
||||
auto action_server = rclcpp_action::create_server<Fibonacci>(
|
||||
node, fibonacci_action_name,
|
||||
[](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
},
|
||||
[&server_goal_handle](std::shared_ptr<GoalHandle> 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<GoalHandle> server_goal_handle = nullptr;
|
||||
auto action_server = rclcpp_action::create_server<Fibonacci>(
|
||||
node, fibonacci_action_name,
|
||||
[](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
},
|
||||
[&server_goal_handle](std::shared_ptr<GoalHandle> 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<Fibonacci::Result>();
|
||||
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<GoalHandle> server_goal_handle = nullptr;
|
||||
auto action_server = rclcpp_action::create_server<Fibonacci>(
|
||||
node, fibonacci_action_name,
|
||||
[](const GoalUUID &, std::shared_ptr<const Fibonacci::Goal>) {
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
|
||||
},
|
||||
[](std::shared_ptr<GoalHandle>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
},
|
||||
[&server_goal_handle](std::shared_ptr<GoalHandle> 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<Fibonacci::Result>();
|
||||
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);
|
||||
}
|
||||
}
|
Loading…
Add table
Add a link
Reference in a new issue