Implemented basic topic subscription-publishing scenario

CMakeLists.txt

@@ -20,7 +20,8 @@ find_package(example_interfaces REQUIRED)
 add_library(
   moveit_middleware_benchmark_lib SHARED
   src/scenarios/scenario_perception_pipeline.cpp
-  src/scenarios/scenario_basic_service_client.cpp)
+  src/scenarios/scenario_basic_service_client.cpp
+  src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp)
 
 ament_target_dependencies(
   moveit_middleware_benchmark_lib
@@ -54,10 +55,32 @@ add_executable(scenario_basic_service_client_benchmark_main
 target_link_libraries(scenario_basic_service_client_benchmark_main
                       PUBLIC moveit_middleware_benchmark_lib)
 
-install(TARGETS scenario_perception_pipeline_benchmark_main
-                scenario_basic_service_client_benchmark_main
+add_executable(scenario_basic_subscription_benchmark_main
+               src/scenario_basic_subscription_benchmark_main.cpp)
+
+target_link_libraries(scenario_basic_subscription_benchmark_main
+                      PUBLIC moveit_middleware_benchmark_lib)
+
+add_executable(scenario_basic_subscription_basic_topic_publisher
+               src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp)
+
+ament_target_dependencies(scenario_basic_subscription_basic_topic_publisher
+                          PUBLIC rclcpp geometry_msgs)
+
+target_include_directories(
+  scenario_basic_subscription_basic_topic_publisher
+  PUBLIC $<INSTALL_INTERFACE:include>
+         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
+
+install(TARGETS scenario_basic_subscription_basic_topic_publisher
         DESTINATION lib/moveit_middleware_benchmark)
 
+install(
+  TARGETS scenario_perception_pipeline_benchmark_main
+          scenario_basic_service_client_benchmark_main
+          scenario_basic_subscription_benchmark_main
+  DESTINATION lib/moveit_middleware_benchmark)
+
 install(DIRECTORY launch config DESTINATION share/moveit_middleware_benchmark)
 
 ament_package()

README.md

@@ -10,6 +10,7 @@ This middleware benchmark tool aims to measure middleware effects on various sce
 
 * [Perception Pipeline](./docs/scenarios/perception_pipeline_benchmark.md)
 * [Basic Service Client Works](./docs/scenarios/basic_service_client_benchmark.md)
+* [Basic Topic Subscription-Publishing](./docs/scenarios/basic_topic_sub_pub.md)
 
 ## Getting Started
 

docs/how_to_run.md

@@ -94,3 +94,7 @@ For instance, the selected test_case includes 20 goal poses. These 20 goals is s
 This benchmark measures the total elapsed time based on the time interval between sending the request by the client to the server and getting the response of server. This benchmark utilizes the [ros2/demos](https://github.com/ros2/demos) packages' [example server](https://github.com/ros2/demos/blob/rolling/demo_nodes_cpp/src/services/add_two_ints_server.cpp).
 
 In this benchmark scenario, the benchmarker node only has client interface. The necessary server for this client is run in [the launch file of this benchmark scenario](../launch/scenario_basic_service_client_benchmark.launch.py). Client sends a request to server and waits for the response from server. Client sends second request to server once the client receives response of first request from client. This actions are repeated `sending_request_number` times. You can configure this `sending_request_number` parameter in [this scenario's launch file]((../launch/scenario_basic_service_client_benchmark.launch.py)).
+
+### [Basic Topic Subscription Publishing Benchmark](scenarios/basic_topic_sub_pub.md)
+
+This benchmark aims to measure the ROS message latency by sending some ROS message which has array section and timestamp section and receiving this message in topic listener side. Firstly, in the topic publisher side, message is created using given `pose_array_size`, `bwnchmarked_topic_name` and `benchmarked_topic_hz`. The topic listener subscribes the topic named `benchmarked_topic_name` to listen this ROS message is published. When message is received from topic publisher side, It's found the message latency by subtracting message timestamp (indicates the time message is published) inside the message callback from current time (indicates the time message is received). Finally, this message latencies are added to total elapsed time. When message number handled achieved `max_received_message_number`, this total elapsed time is used to compare middleware effects in scenario of topic subscription-publishing.

docs/scenarios/basic_topic_sub_pub.md

@@ -0,0 +1,47 @@
+## How To Run Basic Topic Subscription Publishing Benchmark
+
+Firstly, source your ros version. It's suggested to test with rolling version of ROS 2.
+
+For instance, to test with rmw_zenoh, start to zenoh router using following command in the terminal.
+```sh
+# go to your workspace
+cd ws
+# Be sure that ros2 daemon is killed.
+pkill -9 -f ros && ros2 daemon stop
+# Then start zenoh router
+source /opt/ros/rolling/setup.bash
+source install/setup.bash
+export RMW_IMPLEMENTATION=rmw_zenoh_cpp
+ros2 run rmw_zenoh_cpp rmw_zenohd
+```
+
+Select your rmw_implementation as `rmw_zenoh_cpp` and run the perception benchmark launch file in the another terminal.
+```sh
+# go to your workspace
+cd ws
+source /opt/ros/rolling/setup.bash
+source install/setup.bash
+export RMW_IMPLEMENTATION=rmw_zenoh_cpp # select your rmw_implementation to benchmark
+ros2 launch moveit_middleware_benchmark scenario_basic_subscription_benchmark.launch.py
+```
+
+It will be defaultly benchmarked with 6 repetitions. It will be created the json file named `middleware_benchmark_results.json` for benchmarking results after finishing benchmark code execution. You can see the benchmark results in more detail inside this json file.
+
+If you want to customize your benchmark arguments or select different test case, you can use below command.
+
+```shell
+ros2 launch moveit_middleware_benchmark scenario_basic_subscription_benchmark.launch.py benchmark_command_args:="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=1" pose_array_size:=1000 benchmarked_topic_name:="/dummy_benchmark_topic_name" benchmarked_topic_hz:=10 max_received_topic_number:=100
+```
+
+Let's explain some parameters used in benchmark.
+
+| args | explanation |
+| ---- | ----------- |
+| `benchmark_command_args` | you can utilize this parameter to use custom benchmark arguments in your benchmark |
+| `pose_array_size` | In this benchmark, [geometry_msgs/msg/PoseArray](https://docs.ros.org/en/rolling/p/geometry_msgs/interfaces/msg/PoseArray.html) is used. This parameter presents how many poses will be published from [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp). You can set message size sent by [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp) using this parameter. |
+| `benchmarked_topic_hz` | This parameter presents in how many rate the message will be published from [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp). |
+| `max_received_message_number` | This parameter sets how many message to handled by [scenario_basic_subscription benchmarking module](../../src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp). The more message number is handled, the more reliable benchmark results is achieved in `scenario_basic_subscription` scenario. |
+
+## How to benchmark the basic topic subscription publishing works
+
+The main idea here is to firstly save the message timestamp when published from [basic_topic_publisher](../../src/scenarios/basic_topic_sub_pub/basic_topic_publisher.cpp) and then handle this message in [scenario_basic_subscription benchmarking module](../../src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp) by which this message is received. This benchmark module uses [geometry_msgs/msg/PoseArray](https://docs.ros.org/en/rolling/p/geometry_msgs/interfaces/msg/PoseArray.html) which has arrangeable size and timestamp sections in message body. When message is received by [scenario_basic_subscription benchmarking module](../../src/scenarios/basic_topic_sub_pub/scenario_basic_subscription.cpp), the difference between the time message is received and the time message is sent is added to total elapsed time. In the end, this total elapsed time is saved to benchmark results.

include/moveit_middleware_benchmark/scenarios/basic_topic_sub_pub/scenario_basic_subscription.hpp

@@ -0,0 +1,122 @@
+/*********************************************************************
+ * 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 <mutex>
+#include <geometry_msgs/msg/pose_array.hpp>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+namespace moveit
+{
+namespace middleware_benchmark
+{
+
+class ScenarioBasicSubPub
+{
+public:
+  /** \brief Constructor
+   *  \param [in] node The ros node for subscribing the benchmarking topic
+   * and getting the necessary ROS params
+   */
+  ScenarioBasicSubPub(rclcpp::Node::SharedPtr node);
+
+  /** \brief the method to measure the message latency
+   * by handling maximum \e max_received_message_number_ message
+   * \param [in] benchmark_state the google benchmark instance
+   * to save message latency to benchmark results
+   */
+  void runTestCase(benchmark::State& benchmark_state);
+
+  /** \brief the method to subscribe ROS message and add
+   * the message latencies to total elapsed time until
+   * \e max_received_message_number_ message is received
+   * \param [in] msg ROS message
+   */
+  void subCallback(geometry_msgs::msg::PoseArray::SharedPtr msg);
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr sub_;
+  size_t received_message_number_;
+
+  /* max message number to be able to handle */
+  size_t max_received_message_number_;
+
+  /* topic name to subscribe determined topic */
+  std::string benchmarked_topic_name_;
+
+  /* topic publishing hz */
+  int benchmarked_topic_hz_;
+
+  std::condition_variable test_case_cv_;
+  std::mutex is_test_case_finished_mutex_;
+  bool is_test_case_finished_;
+
+  /* total message latency */
+  std::chrono::duration<double> elapsed_time_;
+};
+
+class ScenarioBasicSubPubFixture : public benchmark::Fixture
+{
+public:
+  ScenarioBasicSubPubFixture();
+
+  /** \brief This method runs once each benchmark starts
+   *  \param [in] state
+   */
+  void SetUp(::benchmark::State& /*state*/);
+
+  /** \brief This method runs as soon as each benchmark finishes
+   *  \param [in] state
+   */
+  void TearDown(::benchmark::State& /*state*/);
+
+protected:
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;
+  std::thread node_thread_;
+};
+
+}  // namespace middleware_benchmark
+}  // namespace moveit

launch/scenario_basic_subscription_benchmark.launch.py

@@ -0,0 +1,115 @@
+import os
+from launch import LaunchDescription
+from launch.substitutions import (
+    LaunchConfiguration,
+    EnvironmentVariable,
+    EqualsSubstitution,
+)
+from launch.conditions import IfCondition
+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_received_message_number = int(
+        context.perform_substitution(LaunchConfiguration("max_received_message_number"))
+    )
+
+    pose_array_size = int(
+        context.perform_substitution(LaunchConfiguration("pose_array_size"))
+    )
+
+    topic_publisher = Node(
+        name="topic_publisher",
+        package="moveit_middleware_benchmark",
+        executable="scenario_basic_subscription_basic_topic_publisher",
+        output="log",
+        parameters=[
+            {"pose_array_size": pose_array_size},
+            {"benchmarked_topic_name": benchmarked_topic_name},
+            {"benchmarked_topic_hz": benchmarked_topic_hz},
+        ],
+    )
+
+    zenoh_router = Node(
+        name="zenoh_router",
+        package="rmw_zenoh_cpp",
+        executable="rmw_zenohd",
+        output="both",
+        condition=IfCondition(
+            EqualsSubstitution(
+                EnvironmentVariable("RMW_IMPLEMENTATION", default_value=""),
+                "rmw_zenoh_cpp",
+            ),
+        ),
+    )
+
+    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_received_message_number": max_received_message_number},
+            {"benchmarked_topic_name": benchmarked_topic_name},
+            {"benchmarked_topic_hz": benchmarked_topic_hz},
+        ],
+        on_exit=Shutdown(),
+    )
+
+    return [zenoh_router, 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="10"
+    )
+    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_received_message_number_arg = DeclareLaunchArgument(
+        "max_received_message_number", default_value="1000"
+    )
+    declared_arguments.append(max_received_message_number_arg)
+
+    pose_array_size_arg = DeclareLaunchArgument(
+        "pose_array_size", default_value="1000000"
+    )
+    declared_arguments.append(pose_array_size_arg)
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

scripts/run_all_benchmarks.sh

@@ -8,7 +8,7 @@ helpFunction()
    exit 1 # Exit script after printing help
 }
 
-while getopts "i:m:d:" opt
+while getopts "i:d:" opt
 do
    case "$opt" in
       i ) initial_script="$OPTARG" ;;
@@ -38,3 +38,7 @@ ros2 launch moveit_middleware_benchmark scenario_basic_service_client_benchmark.
 mkdir ${benchmark_results_directory}/scenario_perception_pipeline -p
 ros2 daemon stop
 ros2 launch moveit_middleware_benchmark scenario_perception_pipeline_benchmark.launch.py benchmark_command_args:="--benchmark_out=${benchmark_results_directory}/scenario_perception_pipeline/${RMW_IMPLEMENTATION}.json --benchmark_out_format=json --benchmark_repetitions=6"
+
+mkdir ${benchmark_results_directory}/scenario_basic_subscription_publishing -p
+ros2 daemon stop
+ros2 launch moveit_middleware_benchmark scenario_basic_subscription_benchmark.launch.py benchmark_command_args:="--benchmark_out=${benchmark_results_directory}/scenario_basic_subscription_publishing/${RMW_IMPLEMENTATION}.json --benchmark_out_format=json --benchmark_repetitions=6"