diff --git a/CMakeLists.txt b/CMakeLists.txt index 06d4872..787ecae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(dynmsg REQUIRED) find_package(nav_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(std_msgs REQUIRED) add_executable( scenario_perception_pipeline_benchmark_main @@ -38,7 +39,25 @@ target_include_directories( target_link_libraries(scenario_perception_pipeline_benchmark_main PUBLIC "benchmark::benchmark" ${YAML_CPP_LIBRARIES}) +add_executable( + scenario_basic_subscription_benchmark_main + src/scenario_basic_subscription_benchmark_main.cpp + src/scenarios/scenario_basic_subscription.cpp) + +ament_target_dependencies( + scenario_basic_subscription_benchmark_main PUBLIC + "moveit_ros_planning_interface" "rclcpp" "benchmark" "std_msgs") + +target_include_directories( + scenario_basic_subscription_benchmark_main + PUBLIC $ + $) + +target_link_libraries(scenario_basic_subscription_benchmark_main + PUBLIC "benchmark::benchmark" ${YAML_CPP_LIBRARIES}) + install(TARGETS scenario_perception_pipeline_benchmark_main + scenario_basic_subscription_benchmark_main DESTINATION lib/moveit_middleware_benchmark) install(DIRECTORY launch config DESTINATION share/moveit_middleware_benchmark) diff --git a/include/moveit_middleware_benchmark/scenarios/scenario_basic_subscription.hpp b/include/moveit_middleware_benchmark/scenarios/scenario_basic_subscription.hpp new file mode 100644 index 0000000..46224ed --- /dev/null +++ b/include/moveit_middleware_benchmark/scenarios/scenario_basic_subscription.hpp @@ -0,0 +1,87 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Cihat Kurtuluş Altıparmak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Cihat Kurtuluş Altıparmak + Description: Benchmarking module to compare the effects of middlewares + against topic subscription and publishing + */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace moveit +{ +namespace middleware_benchmark +{ + +class ScenarioBasicSubPub +{ +public: + ScenarioBasicSubPub(rclcpp::Node::SharedPtr node); + + void runTestCase(const int& publishing_topic_number); + void subCallback(std_msgs::msg::String::SharedPtr msg); + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr sub_; + volatile int received_topic_number_; + std::string benchmarked_topic_name_; + int benchmarked_topic_hz_; +}; + +class ScenarioBasicSubPubFixture : public benchmark::Fixture +{ +public: + ScenarioBasicSubPubFixture(); + + void SetUp(::benchmark::State& /*state*/); + + void TearDown(::benchmark::State& /*state*/); + +protected: + rclcpp::Node::SharedPtr node_; + std::shared_ptr executor_; + std::thread node_thread_; + int max_receiving_topic_number_; +}; + +} // namespace middleware_benchmark +} // namespace moveit diff --git a/launch/scenario_basic_subscription_benchmark.launch.py b/launch/scenario_basic_subscription_benchmark.launch.py new file mode 100644 index 0000000..3b8f131 --- /dev/null +++ b/launch/scenario_basic_subscription_benchmark.launch.py @@ -0,0 +1,85 @@ +import os +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + Shutdown, + OpaqueFunction, +) +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + + +def launch_setup(context, *args, **kwargs): + benchmark_command_args = context.perform_substitution( + LaunchConfiguration("benchmark_command_args") + ).split() + + benchmarked_topic_hz = int( + context.perform_substitution(LaunchConfiguration("benchmarked_topic_hz")) + ) + + benchmarked_topic_name = context.perform_substitution( + LaunchConfiguration("benchmarked_topic_name") + ) + + max_receiving_topic_number = int( + context.perform_substitution(LaunchConfiguration("max_receiving_topic_number")) + ) + + topic_publisher = ExecuteProcess( + cmd=[ + [ + f"ros2 topic pub -r {benchmarked_topic_hz} {benchmarked_topic_name} std_msgs/msg/String 'data : 1'", + ] + ], + shell=True, + ) + + benchmark_main_node = Node( + name="benchmark_main", + package="moveit_middleware_benchmark", + executable="scenario_basic_subscription_benchmark_main", + output="both", + arguments=benchmark_command_args, + parameters=[ + {"max_receiving_topic_number": max_receiving_topic_number}, + {"benchmarked_topic_name": benchmarked_topic_name}, + {"benchmarked_topic_hz": benchmarked_topic_hz}, + ], + on_exit=Shutdown(), + ) + + return [topic_publisher, benchmark_main_node] + + +def generate_launch_description(): + declared_arguments = [] + + benchmark_command_args = DeclareLaunchArgument( + "benchmark_command_args", + default_value="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=6", + description="Google Benchmark Tool Arguments", + ) + declared_arguments.append(benchmark_command_args) + + benchmarked_topic_hz_arg = DeclareLaunchArgument( + "benchmarked_topic_hz", default_value="10000" + ) + declared_arguments.append(benchmarked_topic_hz_arg) + + benchmarked_topic_name_arg = DeclareLaunchArgument( + "benchmarked_topic_name", default_value="/benchmarked_topic1" + ) + declared_arguments.append(benchmarked_topic_name_arg) + + max_receiving_topic_number_arg = DeclareLaunchArgument( + "max_receiving_topic_number", default_value="1000000" + ) + declared_arguments.append(max_receiving_topic_number_arg) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/src/scenario_basic_subscription_benchmark_main.cpp b/src/scenario_basic_subscription_benchmark_main.cpp new file mode 100644 index 0000000..a0dde2b --- /dev/null +++ b/src/scenario_basic_subscription_benchmark_main.cpp @@ -0,0 +1,50 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Cihat Kurtuluş Altıparmak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Cihat Kurtuluş Altıparmak + Description: Benchmarking module to compare the effects of middlewares + against perception pipeline + */ + +#include "moveit_middleware_benchmark/scenarios/scenario_basic_subscription.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + benchmark::Initialize(&argc, argv); + benchmark::RunSpecifiedBenchmarks(); + benchmark::Shutdown(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/scenarios/scenario_basic_subscription.cpp b/src/scenarios/scenario_basic_subscription.cpp new file mode 100644 index 0000000..1fa169b --- /dev/null +++ b/src/scenarios/scenario_basic_subscription.cpp @@ -0,0 +1,119 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Cihat Kurtuluş Altıparmak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Cihat Kurtuluş Altıparmak + Description: Benchmarking module to compare the effects of middlewares + against topic subscription and publishing + */ + +#include "moveit_middleware_benchmark/scenarios/scenario_basic_subscription.hpp" + +namespace moveit +{ +namespace middleware_benchmark +{ + +ScenarioBasicSubPub::ScenarioBasicSubPub(rclcpp::Node::SharedPtr node) : node_(node) +{ + received_topic_number_ = 0; + node_->get_parameter("benchmarked_topic_name", benchmarked_topic_name_); + node_->get_parameter("benchmarked_topic_hz", benchmarked_topic_hz_); +} + +void ScenarioBasicSubPub::runTestCase(const int& max_received_topic_number) +{ + RCLCPP_INFO(node_->get_logger(), "Subscribing to topic : %s with hz %d", benchmarked_topic_name_.c_str(), + benchmarked_topic_hz_); + + sub_ = node_->create_subscription( + benchmarked_topic_name_, 10, std::bind(&ScenarioBasicSubPub::subCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), + "Successfully subscribed to topic %s with hz %d! When received msg number is bigger than %d, benchmark " + "will be finished!", + benchmarked_topic_name_.c_str(), benchmarked_topic_hz_, max_received_topic_number); + + while (received_topic_number_ < max_received_topic_number) {} + + RCLCPP_INFO(node_->get_logger(), "Benchmarked test case is finished!"); +} + +void ScenarioBasicSubPub::subCallback(std_msgs::msg::String::SharedPtr msg) +{ + received_topic_number_++; +} + +ScenarioBasicSubPubFixture::ScenarioBasicSubPubFixture() +{ +} + +void ScenarioBasicSubPubFixture::SetUp(::benchmark::State& /*state*/) +{ + if (node_.use_count() == 0) + { + node_ = std::make_shared("test_scenario_basic_sub_pub", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + node_->get_parameter("max_receiving_topic_number", max_receiving_topic_number_); + + executor_ = std::make_shared(); + executor_->add_node(node_); + node_thread_ = std::thread([this]() { executor_->spin(); }); + } +} + +void ScenarioBasicSubPubFixture::TearDown(::benchmark::State& /*state*/) +{ + // Reset ros2 node shared pointers and their service, subscriber, + // and publisher shared ptrs etc. before rclcpp::shutdown is not run. + executor_->cancel(); + if (node_thread_.joinable()) + node_thread_.join(); + + node_.reset(); +} + +BENCHMARK_DEFINE_F(ScenarioBasicSubPubFixture, test_scenario_basic_sub_pub)(benchmark::State& st) +{ + for (auto _ : st) + { + auto sc = ScenarioBasicSubPub(node_); + sc.runTestCase(max_receiving_topic_number_); + } +} + +BENCHMARK_REGISTER_F(ScenarioBasicSubPubFixture, test_scenario_basic_sub_pub); + +} // namespace middleware_benchmark +} // namespace moveit