From 3d6295e76e919260f9ec5babebe7100178e2123a Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Fri, 19 Jul 2024 01:48:47 +0300 Subject: [PATCH 1/2] Made benchmark command arguments passable through ros2 launch args --- ...io_perception_pipeline_benchmark.launch.py | 96 ++++++++++++------- 1 file changed, 61 insertions(+), 35 deletions(-) diff --git a/launch/scenario_perception_pipeline_benchmark.launch.py b/launch/scenario_perception_pipeline_benchmark.launch.py index 473626d..2c620dd 100644 --- a/launch/scenario_perception_pipeline_benchmark.launch.py +++ b/launch/scenario_perception_pipeline_benchmark.launch.py @@ -1,26 +1,29 @@ import os from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, Shutdown from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.conditions import IfCondition from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory -from moveit_configs_utils import MoveItConfigsBuilder from launch.actions import ( DeclareLaunchArgument, ExecuteProcess, + Shutdown, LogInfo, RegisterEventHandler, + OpaqueFunction, ) from launch.event_handlers import OnProcessExit +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder -def generate_launch_description(): - ros2_control_hardware_type = DeclareLaunchArgument( - "ros2_control_hardware_type", - default_value="mock_components", - description="ROS 2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]", +def launch_setup(context, *args, **kwargs): + benchmark_command_args = context.perform_substitution( + LaunchConfiguration("benchmark_command_args") + ).split() + + selected_test_case_index = int( + context.perform_substitution(LaunchConfiguration("selected_test_case_index")) ) moveit_config = ( @@ -121,41 +124,64 @@ def generate_launch_description(): package="moveit_middleware_benchmark", executable="scenario_perception_pipeline_benchmark_main", output="both", - arguments=[ - "--benchmark_out=middleware_benchmark_results.json", - "--benchmark_out_format=json", - ], + arguments=benchmark_command_args, parameters=[ moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, {"use_sim_time": True}, - {"selected_test_case_index": 1}, + {"selected_test_case_index": selected_test_case_index}, ], on_exit=Shutdown(), ) + return [ + move_group_node, + static_tf_node, + robot_state_publisher, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + panda_hand_controller_spawner, + # for https://github.com/ros-controls/ros2_controllers/issues/981 + RegisterEventHandler( + OnProcessExit( + target_action=panda_arm_controller_spawner, + on_exit=[ + LogInfo( + msg="panda_arm_controller_spawner is finished. Now test_scenario_perception_pipeline will start" + ), + benchmark_main_node, + ], + ) + ), + ] + + +def generate_launch_description(): + declared_arguments = [] + + selected_test_case_index_arg = benchmark_command_args = DeclareLaunchArgument( + "selected_test_case_index", + default_value="1", + description="Selected Test Case Number For Perception Pipeline Benchmark", + ) + declared_arguments.append(selected_test_case_index_arg) + + 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) + + ros2_control_hardware_type = DeclareLaunchArgument( + "ros2_control_hardware_type", + default_value="mock_components", + description="ROS 2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]", + ) + declared_arguments.append(ros2_control_hardware_type) + return LaunchDescription( - [ - ros2_control_hardware_type, - static_tf_node, - robot_state_publisher, - move_group_node, - ros2_control_node, - joint_state_broadcaster_spawner, - panda_arm_controller_spawner, - panda_hand_controller_spawner, - # for https://github.com/ros-controls/ros2_controllers/issues/981 - RegisterEventHandler( - OnProcessExit( - target_action=panda_arm_controller_spawner, - on_exit=[ - LogInfo( - msg="panda_arm_controller_spawner is finished. Now test_scenario_perception_pipeline will start" - ), - benchmark_main_node, - ], - ) - ), - ] + declared_arguments + [OpaqueFunction(function=launch_setup)] ) From c1fb384ab1e98069f838ce1c6c38b8e69339848e Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Fri, 19 Jul 2024 16:23:43 +0300 Subject: [PATCH 2/2] Add cli usage of perpeption pipeline benchmark to perception_pipeline_benchmark.md (See https://github.com/CihatAltiparmak/moveit_middleware_benchmark/pull/8#pullrequestreview-2188169768) --- docs/scenarios/perception_pipeline_benchmark.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/docs/scenarios/perception_pipeline_benchmark.md b/docs/scenarios/perception_pipeline_benchmark.md index 5313156..0e543b7 100644 --- a/docs/scenarios/perception_pipeline_benchmark.md +++ b/docs/scenarios/perception_pipeline_benchmark.md @@ -25,7 +25,13 @@ export RMW_IMPLEMENTATION=rmw_zenoh_cpp # select your rmw_implementation to benc ros2 launch moveit_middleware_benchmark scenario_perception_pipeline_benchmark.launch.py ``` -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. +It will be defaultly benchmarked with 6 repetitions using first test_case in [`scenario_perception_pipeline_test_cases.yaml`](../../config/scenario_perception_pipeline_test_cases.yaml) 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_perception_pipeline_benchmark.launch.py benchmark_command_args:="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=1" selected_test_case_index:=0 +``` ## How to benchmark the perception pipeline