diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 91b300f..0bde94d 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -51,6 +51,35 @@ if(BUILD_TESTING) set(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS}) ament_lint_auto_find_test_dependencies() + find_package(performance_test_fixture REQUIRED) + + add_performance_test( + benchmark_lifecycle_client + test/benchmark/benchmark_lifecycle_client.cpp) + if(TARGET benchmark_lifecycle_client) + target_link_libraries(benchmark_lifecycle_client ${PROJECT_NAME}) + ament_target_dependencies(benchmark_lifecycle_client rclcpp) + endif() + add_performance_test( + benchmark_lifecycle_node + test/benchmark/benchmark_lifecycle_node.cpp) + if(TARGET benchmark_lifecycle_node) + target_link_libraries(benchmark_lifecycle_node ${PROJECT_NAME}) + ament_target_dependencies(benchmark_lifecycle_node rclcpp) + endif() + add_performance_test( + benchmark_state + test/benchmark/benchmark_state.cpp) + if(TARGET benchmark_state) + target_link_libraries(benchmark_state ${PROJECT_NAME}) + endif() + add_performance_test( + benchmark_transition + test/benchmark/benchmark_transition.cpp) + if(TARGET benchmark_transition) + target_link_libraries(benchmark_transition ${PROJECT_NAME}) + endif() + ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp) if(TARGET test_lifecycle_node) ament_target_dependencies(test_lifecycle_node diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 2b3bc1f..c04d756 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -25,6 +25,7 @@ ament_lint_auto ament_lint_common mimick_vendor + performance_test_fixture rcutils test_msgs diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp new file mode 100644 index 0000000..e65f0cf --- /dev/null +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp @@ -0,0 +1,307 @@ +// 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 + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_available_states.hpp" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +#include "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +/** + * Benchmarks for measuring performance of a lifecycle_node client. + * + * A service client of a lifecycle node is not a class that's explicitly part of ros2 core, + * but it is the expected interface for interacting with a lifecycle node, so for that reason + * this benchmark exists. + */ + +using namespace std::chrono_literals; +constexpr char const * lifecycle_node_name = "lc_talker"; + +constexpr char const * node_get_state_topic = "/lc_talker/get_state"; +constexpr char const * node_change_state_topic = "/lc_talker/change_state"; +constexpr char const * node_get_available_states_topic = "/lc_talker/get_available_states"; +constexpr char const * node_get_available_transitions_topic = + "/lc_talker/get_available_transitions"; +constexpr char const * node_get_transition_graph_topic = + "/lc_talker/get_transition_graph"; +const lifecycle_msgs::msg::State unknown_state = lifecycle_msgs::msg::State(); +class LifecycleServiceClient : public rclcpp::Node +{ +public: + explicit LifecycleServiceClient(std::string node_name) + : Node(node_name) + { + client_get_available_states_ = this->create_client( + node_get_available_states_topic); + client_get_available_transitions_ = + this->create_client( + node_get_available_transitions_topic); + client_get_transition_graph_ = + this->create_client( + node_get_transition_graph_topic); + client_get_state_ = this->create_client( + node_get_state_topic); + client_change_state_ = this->create_client( + node_change_state_topic); + } + + lifecycle_msgs::msg::State + get_state(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_state_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_get_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Get state request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + return future_result.get()->current_state; + } + + bool + change_state(std::uint8_t transition, std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + request->transition.id = transition; + + if (!client_change_state_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_change_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Change state request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + + return future_result.get()->success; + } + + std::vector + get_available_states(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_available_states_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_get_available_states_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Get available states request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + + return future_result.get()->available_states; + } + + std::vector + get_available_transitions(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_available_transitions_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_get_available_transitions_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Get available transitions request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + + return future_result.get()->available_transitions; + } + + std::vector + get_transition_graph(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_transition_graph_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_get_transition_graph_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Get transition graph request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + + return future_result.get()->available_transitions; + } + +private: + std::shared_ptr> + client_get_available_states_; + std::shared_ptr> + client_get_available_transitions_; + std::shared_ptr> + client_get_transition_graph_; + std::shared_ptr> client_get_state_; + std::shared_ptr> client_change_state_; +}; + +class BenchmarkLifecycleClient : public performance_test_fixture::PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + lifecycle_node = std::make_shared(lifecycle_node_name); + lifecycle_client = std::make_shared("lifecycle_client"); + executor = std::make_shared(); + executor->add_node(lifecycle_node->get_node_base_interface()); + executor->add_node(lifecycle_client->get_node_base_interface()); + spinner_ = std::thread(&rclcpp::executors::SingleThreadedExecutor::spin, executor); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + executor->cancel(); + spinner_.join(); + lifecycle_node.reset(); + lifecycle_client.reset(); + rclcpp::shutdown(); + } + +protected: + std::shared_ptr lifecycle_node; + std::shared_ptr lifecycle_client; + std::shared_ptr executor; + std::thread spinner_; +}; + +BENCHMARK_F(BenchmarkLifecycleClient, get_state)(benchmark::State & state) { + for (auto _ : state) { + const auto lifecycle_state = lifecycle_client->get_state(); + if (lifecycle_state.id != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + const std::string msg = + std::string("Expected state did not match actual: ") + + std::to_string(lifecycle_state.id); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(lifecycle_state); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleClient, change_state)(benchmark::State & state) { + bool success = + lifecycle_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + if (!success) { + state.SkipWithError("Transition to configured state failed"); + } + + reset_heap_counters(); + for (auto _ : state) { + success = + lifecycle_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + if (!success) { + state.SkipWithError("Transition to active state failed"); + } + + success = + lifecycle_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + if (!success) { + state.SkipWithError("Transition to inactive state failed"); + } + } +} + +BENCHMARK_F(BenchmarkLifecycleClient, get_available_states)(benchmark::State & state) { + for (auto _ : state) { + constexpr size_t expected_states = 11u; + const auto states = lifecycle_client->get_available_states(); + if (states.size() != expected_states) { + const std::string msg = + std::string("Expected number of states did not match actual: ") + + std::to_string(states.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(states); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleClient, get_available_transitions)(benchmark::State & state) { + for (auto _ : state) { + constexpr size_t expected_transitions = 2u; + const auto transitions = lifecycle_client->get_available_transitions(); + if (transitions.size() != expected_transitions) { + const std::string msg = + std::string("Expected number of transitions did not match actual: ") + + std::to_string(transitions.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(transitions); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleClient, get_transition_graph)(benchmark::State & state) { + for (auto _ : state) { + constexpr size_t expected_transitions = 25u; + const auto transitions = lifecycle_client->get_transition_graph(); + if (transitions.size() != expected_transitions) { + const std::string msg = + std::string("Expected number of transitions did not match actual: ") + + std::to_string(transitions.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(transitions); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp new file mode 100644 index 0000000..3e9935d --- /dev/null +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp @@ -0,0 +1,157 @@ +// 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 "lifecycle_msgs/msg/state.hpp" +#include "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +/** + * Benchmarks for evaluating rclcpp_lifecycle::LifecycleNode. + * + * Much of the lifecycle node API calls rclcpp::Node and node_interface functions, which are + * likely to be inlined and are not of much value to benchmark. + */ + +class BenchmarkLifecycleNodeConstruction : public performance_test_fixture::PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + rclcpp::shutdown(); + } +}; + +BENCHMARK_F(BenchmarkLifecycleNodeConstruction, construct_lifecycle_node)( + benchmark::State & state) +{ + for (auto _ : state) { + auto node = std::make_shared("node", "ns"); + PERFORMANCE_TEST_FIXTURE_PAUSE_MEASUREMENTS( + state, + { + node.reset(); + }); + } +} + +BENCHMARK_F(BenchmarkLifecycleNodeConstruction, destroy_lifecycle_node)(benchmark::State & state) { + for (auto _ : state) { + std::shared_ptr node(nullptr); + PERFORMANCE_TEST_FIXTURE_PAUSE_MEASUREMENTS( + state, + { + node = std::make_shared("node", "ns"); + }); + node.reset(); + } +} + +class BenchmarkLifecycleNode : public performance_test_fixture::PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + node.reset(); + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node; +}; + +// This is a simple getter, but it crosses over into the rcl library. +BENCHMARK_F(BenchmarkLifecycleNode, get_current_state)(benchmark::State & state) { + for (auto _ : state) { + const auto & lifecycle_state = node->get_current_state(); + if (lifecycle_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + const std::string message = + std::string("Node's current state is: ") + std::to_string(lifecycle_state.id()); + state.SkipWithError(message.c_str()); + } + benchmark::DoNotOptimize(lifecycle_state); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleNode, get_available_states)(benchmark::State & state) { + for (auto _ : state) { + constexpr size_t expected_states = 11u; + const auto lifecycle_states = node->get_available_states(); + if (lifecycle_states.size() != expected_states) { + const std::string msg = std::to_string(lifecycle_states.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(lifecycle_states); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleNode, get_available_transitions)(benchmark::State & state) { + for (auto _ : state) { + constexpr size_t expected_transitions = 25u; + const auto & transitions = node->get_available_transitions(); + if (transitions.size() != expected_transitions) { + const std::string msg = std::to_string(transitions.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(transitions); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleNode, transition_valid_state)(benchmark::State & state) { + const auto & configured = + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + if (configured.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + state.SkipWithError("Transition to configured state failed"); + } + + reset_heap_counters(); + for (auto _ : state) { + const auto & active = + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + if (active.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + state.SkipWithError("Transition to active state failed"); + } + const auto & inactive = + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + if (inactive.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + state.SkipWithError("Transition to inactive state failed"); + } + benchmark::DoNotOptimize(active); + benchmark::DoNotOptimize(inactive); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp new file mode 100644 index 0000000..2e26b08 --- /dev/null +++ b/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp @@ -0,0 +1,39 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp_lifecycle/transition.hpp" + +using PerformanceTest = performance_test_fixture::PerformanceTest; + +// These are relatively quick benchmarks, so it would be difficult to separate construction from +// destruction +BENCHMARK_F(PerformanceTest, construct_destruct_state)(benchmark::State & state) +{ + for (auto _ : state) { + rclcpp_lifecycle::State lifecycle_state(1, "state"); + benchmark::DoNotOptimize(lifecycle_state); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(PerformanceTest, copy_destruct_state)(benchmark::State & state) +{ + rclcpp_lifecycle::State lifecycle_state(1, "state"); + for (auto _ : state) { + rclcpp_lifecycle::State state_copy(lifecycle_state); + benchmark::DoNotOptimize(state_copy); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp new file mode 100644 index 0000000..9403ce6 --- /dev/null +++ b/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp @@ -0,0 +1,39 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp_lifecycle/transition.hpp" + +using PerformanceTest = performance_test_fixture::PerformanceTest; + +// These are relatively quick benchmarks, so it would be difficult to separate construction from +// destruction +BENCHMARK_F(PerformanceTest, construct_destruct_transition)(benchmark::State & state) +{ + for (auto _ : state) { + rclcpp_lifecycle::Transition transition(1, "transition"); + benchmark::DoNotOptimize(transition); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(PerformanceTest, copy_destruct_transition)(benchmark::State & state) +{ + rclcpp_lifecycle::Transition transition(1, "transition"); + for (auto _ : state) { + rclcpp_lifecycle::Transition transition_copy(transition); + benchmark::DoNotOptimize(transition_copy); + benchmark::ClobberMemory(); + } +}