Add benchmarks for simple topic subscription publishing scenarios

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 $<INSTALL_INTERFACE:include>
+         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+
+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)

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 <rclcpp/rclcpp.hpp>
+#include <benchmark/benchmark.h>
+#include <memory>
+#include <std_msgs/msg/string.hpp>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+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<std_msgs::msg::String>::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<rclcpp::executors::SingleThreadedExecutor> executor_;
+  std::thread node_thread_;
+  int max_receiving_topic_number_;
+};
+
+}  // namespace middleware_benchmark
+}  // namespace moveit

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)]
+    )

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;
+}

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<std_msgs::msg::String>(
+      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<rclcpp::Node>("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<rclcpp::executors::SingleThreadedExecutor>();
+    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