Ros2 benchmark qa tool

tools/benchmark/README.md

@@ -0,0 +1,176 @@
+# Stereolabs ZED Camera - ROS 2 Wrapper Benchmark Tool
+
+This package provides benchmarking tools for evaluating the performance of ZED ROS 2 integrations. It includes one benchmark node: `zed_topic_benchmark`, which measures the throughput and stability of published ZED topics. It is also possible to complete the test with the ZED SDK Hardware Performance Monitor. It monitors system resource usage such as CPU and GPU while the ZED pipeline is running. Together, this tool helps assess both communication performance and hardware load under realistic operating conditions.
+
+## ZED Topic Benchmark node
+
+It can be used to test frequency and bandwidth of topics and eventually plot the realtime values.
+
+For each topic the following information will be available:
+
+* Topic type
+* Real time frequency
+* Window average frequency
+* Topic size
+* Real time topic bandwidth
+* Window average topic bandwidth
+
+The node publishes a message on the topic `<name_of_the_topic_to_test>_stats` containing all the information to be eventually plotted.
+
+When the node is killed, results are published in a text file, containing : 
+
+* Topic name and type
+* Window average topic frequency (with standard deviation)
+* Window average topic bandwidth
+
+**Note:** This tool is not available for Foxy distribution because it misses an important feature required to subscribe to "generic topics".
+
+### Parameters
+
+* `topic_name`: name of the topic to test
+* `avg_win_size`: size of the mobile window for the calculation of the average. [Default: `500`]
+* `results_file_path`: path file on which benchmark results are published.  
+
+### Custom message
+
+The node published a message on the topic `<name_of_the_topic_to_test>_stats` containing the previous information to be plotted.
+The topic is a custom message of type `BenchmarkStatsStamped` defined as:
+
+```
+# Standard Header
+std_msgs/Header header
+
+# Instant Frequency
+float32 topic_freq
+# Average Frequency
+float32 topic_avg_freq
+
+# Instant Bandwidth
+float32 topic_bw
+# Average Bandwidth
+float32 topic_avg_bw
+```
+
+### Usage
+
+Set up the ROS2 wrapper parameters with the desired configuration. Run the ZED ROS2 wrapper in one terminal. Then, open a new terminal console and start the benchmark node:
+
+```bash
+$ ros2 run zed_benchmark zed_topic_benchmark --ros-args -p topic_name:=<name_of_the_topic_to_test> -p results_file_path:=<path> 
+```
+
+for example:
+
+In one terminal, run the ZED Ros2 wrapper with a ZED 2i camera: 
+
+```bash
+$ ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i
+```
+
+Then, in another terminal, set up the topic benchmark node to monitor the image topic (capture performance benchmark):
+
+```bash
+$ ros2 run zed_benchmark zed_topic_benchmark --ros-args -p topic_name:=/zed2i/zed_node/rgb/color/rect/image -p results_file_path:=benchmark_image_topic.txt
+```
+
+The node will print all the topic information on the console:
+
+```bash
+[INFO] [1665764376.862143962] [topic_benchmark]: ***** Benchmark parameters *****
+[INFO] [1665764376.862355022] [topic_benchmark]: * Topic name: /zed2i/zed_node/rgb/color/rect/image
+[INFO] [1665764376.862462857] [topic_benchmark]: Average window size: 500
+[INFO] [1665764376.862476172] [topic_benchmark]: *** START BENCHMARK ***
+[INFO] [1665764376.863484009] [topic_benchmark]: Advertised on topic: /zed/zed_node/rgb/color/rect/image_stats
+[INFO] [1665764376.863484254] [topic_benchmark]: Average window size: 500
+[INFO] [1665764376.863484126] [topic_benchmark]: ROS Log: 0
+[INFO] [1665764376.863484785] [topic_benchmark]: Results File path: benchmark_image_topic.txt
+[INFO] [1665764377.363430211] [topic_benchmark]: Found topic: '/zed/zed_node/rgb/color/rect/image' of type: 'sensor_msgs/msg/Image'
+#119 - Freq: 6.01 Hz (Avg: 13.49 Hz) - Bandwidth: 42.28 Mbps (Avg: 94.87 Mbps) - Msg size: 0.88 MB
+```
+When stopped, results can be found directly in the provided input text file: 
+
+```json
+Topic Name: /zed2i/zed_node/rgb/color/rect/image
+Topic Type: sensor_msgs/msg/Image
+Average Frequency: 10.5915
+Average Frequency Standard Deviation: 0.755081
+Average Bandwidth: 148.293
+-----------------------------
+```
+
+## Completing the benchmark test with Hardware Load 
+
+This benchmark can be used to evaluate the CPU, GPU, and RAM usage generated by the ZED ROS 2 wrapper. For accurate and reproducible results, it is recommended to run the benchmark with only the wrapper node active, ensuring that resource usage from other processes does not affect the measurements.
+
+When the node is killed, results are published in a text file, containing : 
+
+* Average CPU load.
+* Average GPU load.
+* Overall Ram load.
+
+### Usage
+
+To activate the ZED SDK Hardware Load monitoring process, simply run this command ahead of launching the ROS2 wrapper:
+
+```bash
+$ export ZED_SDK_BENCHMARK_MODE=<path/to/results/file/file_name>.json
+```
+
+for example:
+
+In one terminal, run the ZED Ros2 wrapper with a ZED 2i camera and the Performance Monitoring enabled: 
+
+```bash
+$ export ZED_SDK_BENCHMARK_MODE=hardware_load_results.json
+$ ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i
+```
+
+Then, in another terminal, set up the topic benchmark node to monitor the image topic (capture performance benchmark):
+
+```bash
+$ ros2 run zed_benchmark zed_topic_benchmark --ros-args -p topic_name:=/zed/zed_node/rgb/color/rect/image -p results_file_path:=benchmark_image_topic.txt
+```
+
+When the wrapper is killed, the results are published in the terminal and results report can be found in the dedicated json file: 
+
+```bash
+[component_container_isolated-1] Performance Report (sorted by CPU time, some elements are run in parallel):
+[component_container_isolated-1] EVENT NAME        CPU MEAN (ms)  CPU MED (ms)  CPU STD (ms)   Count
+[component_container_isolated-1] -------------------------------------------------------------------
+[component_container_isolated-1] Open                   3752.604      3752.604         0.000       1
+[component_container_isolated-1] Grab_SN41758409          32.007        27.573         8.660     629
+[component_container_isolated-1] RetrieveObjects           3.379         3.236         1.503     625
+[component_container_isolated-1] RetrieveImage             1.460         1.087         0.741     628
+[component_container_isolated-1] GetPosition               0.021         0.017         0.053    1256
+[component_container_isolated-1] 
+[component_container_isolated-1] Mean grab frequency: 28.3 Hz (median period: 35.4ms)
+[component_container_isolated-1] 
+[component_container_isolated-1] Hardware Load:
+[component_container_isolated-1] RESOURCE             MEAN              MEDIAN            STDDEV            COUNT 
+[component_container_isolated-1] ---------------------------------------------------------------------------------
+[component_container_isolated-1] CPU Load             136.9 %           114.3 %           107.3 %           2803  
+[component_container_isolated-1] GPU Load             69.9 %            80.0 %            32.6 %            2803  
+[component_container_isolated-1] Unified Memory       829.9 MB (5%)     946.7 MB (6%)     286.5 MB (2%)     2803  
+```
+
+## Advanced
+
+### Composition 
+
+The package provides also ROS 2 components called `stereolabs::TopicBenchmarkComponent` to be used with [Composition](https://docs.ros.org/en/humble/Tutorials/Intermediate/Composition.html) to test [Intra Process Communication (IPC)](https://design.ros2.org/articles/intraprocess_communications.html) performances.
+
+In the launch folder of this package, an example launch file (__zed_wrapper_benchmark_test.launch.py__) template is provided. It is based on the [zed_multi_camera](https://github.com/stereolabs/zed-ros2-examples/tree/master/tutorials/zed_multi_camera) example and is intended to enable benchmarking of the ZED ROS 2 wrapper in both single-camera and multi-camera configurations using composition. This launcher can be used as a template for creating custom benchmark tests. The benchmark component nodes are exposed with the same parameters described in the previous sections.
+
+The example launcher is designed to offer modularity and automation in the selection of topics to be monitored during benchmarking. It accepts a `ros_params_override_path` input parameter, which specifies the path to a ROS 2 wrapper YAML configuration file. This file allows users to configure the ROS 2 wrapper as needed for their test scenario. The launcher starts the multi-camera ROS 2 wrapper setup using the provided configuration and automatically enables benchmarking for topics corresponding to the activated SDK modules (e.g. Depth, Positional Tracking, Object Detection) across all available cameras.
+
+By default, standard image capture topics are always monitored. If depth is enabled for example, point cloud topics are automatically included. Similarly, enabling positional tracking or object detection will activate monitoring of their respective topics. Users may also add additional topics as required for specific test cases. The launcher is designed to cover the most commonly monitored topics for performance evaluation while remaining extensible.
+
+An additional node, `performance_test_duration_node`, is also available and is enabled in the template launcher when the `performance_test_duration` parameter is set to a value greater than zero (default value). This parameter allows users to define a fixed duration for the benchmark. In this mode, the node acts as a supervisory process and automatically terminates all ROS 2 processes started by the launcher once the specified duration (`performance_test_duration` seconds) has elapsed. Benchmark results can then also be retrieved in the provided text files.
+
+In order to complete the test with the SDK Hardware Performance Monitor, just export the `ZED_SDK_ENABLE_PERFORMANCE_TIMING` variable with a dedicated json file for the results report ahead of launching the composition wrapper launcher.
+
+### Test Script
+
+The package also provides a Bash script template that leverages the composition launcher together with the `performance_test_duration` node to run batches of successive benchmark tests automatically. This approach enables systematic performance evaluation across different camera setups (single or multi-camera) and wrapper configurations.
+
+The template script is located in the `scripts` directory of the package (*ros_wrapper_benchmark_test.sh*) and can be easily adapted to suit specific testing requirements. In its default form, the script defines three benchmark runs using 1, 2, and 4 cameras, all sharing the same wrapper configuration, providing a simple example of automated and repeatable performance testing.

tools/zed_benchmark/CMakeLists.txt → tools/benchmark/zed_benchmark/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(zed_topic_benchmark)
+project(zed_benchmark)
 
 ## Generate symbols for IDE indexer
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
@@ -64,15 +64,17 @@ if(${FOUND_ROS2_DISTRO} STREQUAL "humble" OR ${FOUND_ROS2_DISTRO} STREQUAL "iron
   set(DEPENDENCIES
     rclcpp
     rclcpp_components
-    zed_topic_benchmark_component
+    zed_benchmark_components
   )
 
   find_package(ament_cmake_auto REQUIRED)
   ament_auto_find_build_dependencies()
 
   find_package(rclcpp REQUIRED)
   find_package(rclcpp_components REQUIRED)
-  find_package(zed_topic_benchmark_component REQUIRED)
+  find_package(zed_benchmark_components REQUIRED)
+
+  find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED)
 
   if(BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
@@ -85,18 +87,40 @@ if(${FOUND_ROS2_DISTRO} STREQUAL "humble" OR ${FOUND_ROS2_DISTRO} STREQUAL "iron
   file(GLOB_RECURSE extra_files ${CMAKE_SOURCE_DIR}/*)
   add_custom_target(${PROJECT_NAME}_files SOURCES ${extra_files})
   ###############################################################################
+
+  ## INCLUDES and LIBS
+  include_directories(
+    ${CUDA_INCLUDE_DIRS}
+  )
+
+  link_directories(${CUDA_LIBRARY_DIRS})
 
   ## Sources
-  set(SRC_CONT src/topic_benchmark_container.cpp)
+  set(SRC_CONT 
+  	src/topic_benchmark_container.cpp
+  )
+
 
-  ## Build
-  add_executable(${PROJECT_NAME} ${SRC_CONT})
-  ament_target_dependencies(${PROJECT_NAME}
+
+  ## Build Topic Benchmark node
+  add_executable(zed_topic_benchmark ${SRC_CONT})
+  ament_target_dependencies(zed_topic_benchmark
     ${DEPENDENCIES}
+    zed_benchmark_components
   )
-
-  # Install executable
-  install(TARGETS ${PROJECT_NAME}
+
+  target_link_libraries(zed_topic_benchmark 
+  zed_benchmark_components::zed_topic_benchmark_component
+  )
+
+
+  ## Build Performance Test Duration Node
+
+  add_executable(performance_test_duration_node src/performance_test_duration_node.cpp)
+  ament_target_dependencies(performance_test_duration_node  rclcpp)
+
+  ## Install executable
+  install(TARGETS zed_topic_benchmark performance_test_duration_node
     ARCHIVE DESTINATION lib
     LIBRARY DESTINATION lib
     RUNTIME DESTINATION lib/${PROJECT_NAME}
@@ -105,6 +129,8 @@ if(${FOUND_ROS2_DISTRO} STREQUAL "humble" OR ${FOUND_ROS2_DISTRO} STREQUAL "iron
   # Install LAUNCH files
   install(DIRECTORY
     launch
+    config
+    scripts
     DESTINATION share/${PROJECT_NAME}
   )
 

tools/benchmark/zed_benchmark/config/ros_wrapper_performance_test_config.yaml

@@ -0,0 +1,40 @@
+# config/common_stereo.yaml
+# Common parameters to Stereolabs ZED Stereo cameras
+
+---
+/**:
+    ros__parameters:
+
+        general:
+            pub_resolution: 'CUSTOM' # The resolution used for image and depth map publishing. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission
+            pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM'
+            pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images
+            camera_model: 'zedx'
+            grab_resolution: 'HD1200' # The native camera grab resolution. 'HD1200', 'HD1080', 'SVGA', 'AUTO'
+            grab_frame_rate: 30 # ZED SDK internal grabbing rate (HD1200/HD1080: 60, 30, 15 - SVGA: 120, 60, 30, 15)
+
+        depth: # Only stereo cameras
+            depth_mode: 'NEURAL' # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...)
+            point_cloud_freq: 30.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `pub_frame_rate` value)
+            point_cloud_res: 'COMPACT' # The resolution used for point cloud publishing - 'COMPACT'-Standard resolution. Optimizes processing and bandwidth, 'REDUCED'-Half resolution. Low processing and bandwidth requirements
+            depth_stabilization: 30 # [DYNAMIC] - enable/disable depth stabilization. 0: disabled, 1: enabled. Note: this parameter is not available for the ZED camera
+
+        pos_tracking:
+            pos_tracking_enabled: true # True to enable positional tracking from start
+            pos_tracking_mode: 'GEN_3' # Matches the ZED SDK setting: 'GEN_1', 'GEN_2'
+            imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
+            publish_tf: true # [usually overwritten by launch file] publish `odom -> camera_link` TF
+            publish_map_tf: true # [usually overwritten by launch file] publish `map -> odom` TF
+
+        object_detection:
+            od_enabled: true # True to enable Object Detection
+            enable_tracking: true # Whether the object detection system includes object tracking capabilities across a sequence of images.
+            detection_model: 'MULTI_CLASS_BOX_FAST' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE', 'CUSTOM_YOLOLIKE_BOX_OBJECTS'
+            max_range: 20.0 # [m] Upper depth range for detections.The value cannot be greater than 'depth.max_depth'
+            filtering_mode: 'NMS3D' # Filtering mode that should be applied to raw detections: 'NONE', 'NMS3D', 'NMS3D_PER_CLASS'
+            prediction_timeout: 2.0 # During this time [sec], the object will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions
+            allow_reduced_precision_inference: false # Allow inference to run at a lower precision to improve runtime and memory usage
+            # Other parameters are defined in the 'object_detection.yaml' and 'custom_object_detection.yaml' files
+
+        debug:
+            disable_nitros: true # If available, disable NITROS usage for debugging and testing purposes