mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-09-12 19:20:20 +08:00
Add ob_benchmark_node tool and benchmark examples
This commit is contained in:
jj
@@ -218,6 +218,7 @@ add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp)
|
||||
add_orbbec_executable(multi_save_rgbir_node tools/multi_save_rgbir_node.cpp)
|
||||
add_orbbec_executable(metadata_save_files_node tools/metadata_save_files.cpp)
|
||||
add_orbbec_executable(metadata_export_files_node tools/metadata_export_files.cpp)
|
||||
add_orbbec_executable(ob_benchmark_node tools/ob_benchmark.cpp)
|
||||
|
||||
add_library(frame_latency SHARED tools/frame_latency.cpp)
|
||||
target_include_directories(frame_latency PUBLIC ${COMMON_INCLUDE_DIRS})
|
||||
@@ -229,8 +230,19 @@ rclcpp_components_register_node(frame_latency
|
||||
EXECUTABLE frame_latency_node
|
||||
)
|
||||
|
||||
add_library(start_benchmark SHARED tools/start_benchmark.cpp)
|
||||
target_include_directories(start_benchmark PUBLIC ${COMMON_INCLUDE_DIRS})
|
||||
target_link_libraries(start_benchmark ${COMMON_LIBRARIES})
|
||||
ament_target_dependencies(start_benchmark ${dependencies})
|
||||
|
||||
rclcpp_components_register_node(start_benchmark
|
||||
PLUGIN "orbbec_camera::tools::StartBenchmark"
|
||||
EXECUTABLE start_benchmark_node
|
||||
)
|
||||
|
||||
# Install rules
|
||||
install(TARGETS ${PROJECT_NAME} frame_latency
|
||||
start_benchmark
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
@@ -256,6 +268,7 @@ install(TARGETS list_devices_node
|
||||
multi_save_rgbir_node
|
||||
metadata_save_files_node
|
||||
metadata_export_files_node
|
||||
ob_benchmark_node
|
||||
DESTINATION lib/${PROJECT_NAME}/)
|
||||
|
||||
if (BUILD_TESTING)
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
{
|
||||
"start_benchmark_params": {
|
||||
"camera_name": [
|
||||
"camera_01",
|
||||
"camera_02",
|
||||
"camera_03",
|
||||
"camera_04"
|
||||
],
|
||||
"process_name": "component_conta",
|
||||
"switch_cycle": 60,
|
||||
"test_cycle": 1,
|
||||
"skip_number": 30
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1,265 @@
|
||||
import os
|
||||
import yaml
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import PushRosNamespace, ComposableNodeContainer, Node
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
from launch.conditions import UnlessCondition
|
||||
from launch_ros.actions import LoadComposableNodes
|
||||
|
||||
def load_yaml(file_path):
|
||||
with open(file_path, 'r') as f:
|
||||
return yaml.safe_load(f)
|
||||
|
||||
|
||||
def merge_params(default_params, yaml_params):
|
||||
for key, value in yaml_params.items():
|
||||
if key in default_params:
|
||||
default_params[key] = value
|
||||
return default_params
|
||||
|
||||
|
||||
def convert_value(value):
|
||||
if isinstance(value, str):
|
||||
try:
|
||||
return int(value)
|
||||
except ValueError:
|
||||
pass
|
||||
try:
|
||||
return float(value)
|
||||
except ValueError:
|
||||
pass
|
||||
if value.lower() == 'true':
|
||||
return True
|
||||
elif value.lower() == 'false':
|
||||
return False
|
||||
return value
|
||||
|
||||
|
||||
def load_parameters(context, args):
|
||||
default_params = {arg.name: LaunchConfiguration(arg.name).perform(context) for arg in args}
|
||||
config_file_path = LaunchConfiguration('config_file_path').perform(context)
|
||||
if config_file_path:
|
||||
yaml_params = load_yaml(config_file_path)
|
||||
default_params = merge_params(default_params, yaml_params)
|
||||
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
|
||||
return {
|
||||
key: (value if key in skip_convert else convert_value(value))
|
||||
for key, value in default_params.items()
|
||||
}
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
args = [
|
||||
DeclareLaunchArgument('camera_name', default_value='camera'),
|
||||
DeclareLaunchArgument('depth_registration', default_value='true'),
|
||||
DeclareLaunchArgument('serial_number', default_value=''),
|
||||
DeclareLaunchArgument('usb_port', default_value=''),
|
||||
DeclareLaunchArgument('device_num', default_value='1'),
|
||||
DeclareLaunchArgument('uvc_backend', default_value='libuvc'),#libuvc or v4l2
|
||||
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
|
||||
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
|
||||
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
|
||||
DeclareLaunchArgument('cloud_frame_id', default_value=''),
|
||||
DeclareLaunchArgument('connection_delay', default_value='10'),
|
||||
DeclareLaunchArgument('color_width', default_value='0'),
|
||||
DeclareLaunchArgument('color_height', default_value='0'),
|
||||
DeclareLaunchArgument('color_fps', default_value='0'),
|
||||
DeclareLaunchArgument('color_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_color', default_value='true'),
|
||||
DeclareLaunchArgument('color_qos', default_value='default'),
|
||||
DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
|
||||
DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
|
||||
DeclareLaunchArgument('color_exposure', default_value='-1'),
|
||||
DeclareLaunchArgument('color_gain', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'),
|
||||
DeclareLaunchArgument('color_white_balance', default_value='-1'),
|
||||
DeclareLaunchArgument('color_ae_max_exposure', default_value='-1'),
|
||||
DeclareLaunchArgument('color_brightness', default_value='-1'),
|
||||
DeclareLaunchArgument('depth_width', default_value='0'),
|
||||
DeclareLaunchArgument('depth_height', default_value='0'),
|
||||
DeclareLaunchArgument('depth_fps', default_value='0'),
|
||||
DeclareLaunchArgument('depth_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_depth', default_value='true'),
|
||||
DeclareLaunchArgument('depth_qos', default_value='default'),
|
||||
DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
|
||||
DeclareLaunchArgument('left_ir_width', default_value='0'),
|
||||
DeclareLaunchArgument('left_ir_height', default_value='0'),
|
||||
DeclareLaunchArgument('left_ir_fps', default_value='0'),
|
||||
DeclareLaunchArgument('left_ir_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_left_ir', default_value='false'),
|
||||
DeclareLaunchArgument('left_ir_qos', default_value='default'),
|
||||
DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'),
|
||||
DeclareLaunchArgument('right_ir_width', default_value='0'),
|
||||
DeclareLaunchArgument('right_ir_height', default_value='0'),
|
||||
DeclareLaunchArgument('right_ir_fps', default_value='0'),
|
||||
DeclareLaunchArgument('right_ir_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_right_ir', default_value='false'),
|
||||
DeclareLaunchArgument('right_ir_qos', default_value='default'),
|
||||
DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
|
||||
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
|
||||
DeclareLaunchArgument('ir_exposure', default_value='-1'),
|
||||
DeclareLaunchArgument('ir_gain', default_value='-1'),
|
||||
DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'),
|
||||
DeclareLaunchArgument('ir_brightness', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'),
|
||||
DeclareLaunchArgument('enable_accel', default_value='false'),
|
||||
DeclareLaunchArgument('accel_rate', default_value='200hz'),
|
||||
DeclareLaunchArgument('accel_range', default_value='4g'),
|
||||
DeclareLaunchArgument('enable_gyro', default_value='false'),
|
||||
DeclareLaunchArgument('gyro_rate', default_value='200hz'),
|
||||
DeclareLaunchArgument('gyro_range', default_value='1000dps'),
|
||||
DeclareLaunchArgument('liner_accel_cov', default_value='0.01'),
|
||||
DeclareLaunchArgument('angular_vel_cov', default_value='0.01'),
|
||||
DeclareLaunchArgument('publish_tf', default_value='true'),
|
||||
DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
|
||||
DeclareLaunchArgument('ir_info_url', default_value=''),
|
||||
DeclareLaunchArgument('color_info_url', default_value=''),
|
||||
DeclareLaunchArgument('log_level', default_value='none'),
|
||||
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
|
||||
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hardware_d2d', default_value='true'),
|
||||
DeclareLaunchArgument('enable_ldp', default_value='true'),
|
||||
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
|
||||
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
|
||||
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
|
||||
DeclareLaunchArgument('sync_mode', default_value='standalone'),
|
||||
DeclareLaunchArgument('depth_delay_us', default_value='0'),
|
||||
DeclareLaunchArgument('color_delay_us', default_value='0'),
|
||||
DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
|
||||
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
|
||||
DeclareLaunchArgument('trigger_out_enabled', default_value='true'),
|
||||
DeclareLaunchArgument('frames_per_trigger', default_value='2'),
|
||||
DeclareLaunchArgument('software_trigger_period', default_value='33'), # ms
|
||||
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
|
||||
DeclareLaunchArgument('ordered_pc', default_value='false'),
|
||||
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
|
||||
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
|
||||
DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hardware_noise_removal_filter', default_value='true'),
|
||||
DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'),
|
||||
DeclareLaunchArgument('enable_spatial_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'),
|
||||
DeclareLaunchArgument('decimation_filter_scale', default_value='-1'),
|
||||
DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'),
|
||||
DeclareLaunchArgument('threshold_filter_max', default_value='-1'),
|
||||
DeclareLaunchArgument('threshold_filter_min', default_value='-1'),
|
||||
DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'),
|
||||
DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'),
|
||||
DeclareLaunchArgument('spatial_filter_alpha', default_value='-1.0'),
|
||||
DeclareLaunchArgument('spatial_filter_diff_threshold', default_value='-1'),
|
||||
DeclareLaunchArgument('spatial_filter_magnitude', default_value='-1'),
|
||||
DeclareLaunchArgument('spatial_filter_radius', default_value='-1'),
|
||||
DeclareLaunchArgument('temporal_filter_diff_threshold', default_value='-1.0'),
|
||||
DeclareLaunchArgument('temporal_filter_weight', default_value='-1.0'),
|
||||
DeclareLaunchArgument('hole_filling_filter_mode', default_value=''),
|
||||
DeclareLaunchArgument('hdr_merge_exposure_1', default_value='-1'),
|
||||
DeclareLaunchArgument('hdr_merge_gain_1', default_value='-1'),
|
||||
DeclareLaunchArgument('hdr_merge_exposure_2', default_value='-1'),
|
||||
DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'),
|
||||
DeclareLaunchArgument('align_mode', default_value='SW'),
|
||||
DeclareLaunchArgument('diagnostic_period', default_value='1.0'),
|
||||
DeclareLaunchArgument('enable_laser', default_value='true'),
|
||||
DeclareLaunchArgument('depth_precision', default_value=''),
|
||||
DeclareLaunchArgument('device_preset', default_value='Default'),
|
||||
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
|
||||
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
|
||||
DeclareLaunchArgument('enable_sync_host_time', default_value='true'),
|
||||
DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system
|
||||
DeclareLaunchArgument('enable_color_undistortion', default_value='false'),
|
||||
DeclareLaunchArgument('config_file_path', default_value=''),
|
||||
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
|
||||
DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
|
||||
DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
|
||||
DeclareLaunchArgument('disparity_range_mode', default_value='-1'),
|
||||
DeclareLaunchArgument('disparity_search_offset', default_value='-1'),
|
||||
DeclareLaunchArgument('disparity_offset_config', default_value='false'),
|
||||
DeclareLaunchArgument('offset_index0', default_value='-1'),
|
||||
DeclareLaunchArgument('offset_index1', default_value='-1'),
|
||||
DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'), # full_frame、color_frame、ANY or disable
|
||||
DeclareLaunchArgument('interleave_ae_mode', default_value='laser'), # 'hdr' or 'laser'
|
||||
DeclareLaunchArgument('interleave_frame_enable', default_value='false'),
|
||||
DeclareLaunchArgument('interleave_skip_enable', default_value='false'),
|
||||
DeclareLaunchArgument('interleave_skip_index', default_value='1'), # 0:skip pattern ir 1: skip flood ir
|
||||
|
||||
DeclareLaunchArgument('hdr_index1_laser_control', default_value='1'),#interleave_hdr_param
|
||||
DeclareLaunchArgument('hdr_index1_depth_exposure', default_value='1'),
|
||||
DeclareLaunchArgument('hdr_index1_depth_gain', default_value='16'),
|
||||
DeclareLaunchArgument('hdr_index1_ir_brightness', default_value='20'),
|
||||
DeclareLaunchArgument('hdr_index1_ir_ae_max_exposure', default_value='2000'),
|
||||
DeclareLaunchArgument('hdr_index0_laser_control', default_value='1'),
|
||||
DeclareLaunchArgument('hdr_index0_depth_exposure', default_value='7500'),
|
||||
DeclareLaunchArgument('hdr_index0_depth_gain', default_value='16'),
|
||||
DeclareLaunchArgument('hdr_index0_ir_brightness', default_value='60'),
|
||||
DeclareLaunchArgument('hdr_index0_ir_ae_max_exposure', default_value='10000'),
|
||||
|
||||
DeclareLaunchArgument('laser_index1_laser_control', default_value='0'),#interleave_laser_param
|
||||
DeclareLaunchArgument('laser_index1_depth_exposure', default_value='3000'),
|
||||
DeclareLaunchArgument('laser_index1_depth_gain', default_value='16'),
|
||||
DeclareLaunchArgument('laser_index1_ir_brightness', default_value='60'),
|
||||
DeclareLaunchArgument('laser_index1_ir_ae_max_exposure', default_value='17000'),
|
||||
DeclareLaunchArgument('laser_index0_laser_control', default_value='1'),
|
||||
DeclareLaunchArgument('laser_index0_depth_exposure', default_value='3000'),
|
||||
DeclareLaunchArgument('laser_index0_depth_gain', default_value='16'),
|
||||
DeclareLaunchArgument('laser_index0_ir_brightness', default_value='60'),
|
||||
DeclareLaunchArgument('laser_index0_ir_ae_max_exposure', default_value='30000'),
|
||||
DeclareLaunchArgument('use_intra_process_comms', default_value='false'),
|
||||
DeclareLaunchArgument('attach_component_container_enable', default_value='false'),
|
||||
DeclareLaunchArgument('attach_component_container_name', default_value='orbbec_container'),
|
||||
|
||||
]
|
||||
|
||||
def get_params(context, args):
|
||||
return [load_parameters(context, args)]
|
||||
|
||||
def create_node_action(context, args):
|
||||
params = get_params(context, args)
|
||||
ros_distro = os.environ.get("ROS_DISTRO", "humble")
|
||||
if ros_distro == "foxy":
|
||||
return [
|
||||
Node(
|
||||
package="orbbec_camera",
|
||||
executable="orbbec_camera_node",
|
||||
name="ob_camera_node",
|
||||
namespace=LaunchConfiguration("camera_name"),
|
||||
parameters=params,
|
||||
output="screen",
|
||||
)
|
||||
]
|
||||
else:
|
||||
attach_to_shared_component_container_arg = LaunchConfiguration('attach_to_shared_component_container', default=False)
|
||||
component_container_name_arg = LaunchConfiguration('component_container_name', default='orbbec_container')
|
||||
|
||||
orbbec_container = Node(
|
||||
name=component_container_name_arg,
|
||||
package='rclcpp_components',
|
||||
executable='component_container_mt',
|
||||
output='screen',
|
||||
condition=UnlessCondition(attach_to_shared_component_container_arg)
|
||||
)
|
||||
return [
|
||||
# orbbec_container,
|
||||
LoadComposableNodes(
|
||||
target_container=component_container_name_arg,
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
namespace=LaunchConfiguration("camera_name"),
|
||||
name=LaunchConfiguration("camera_name"),
|
||||
package='orbbec_camera',
|
||||
plugin='orbbec_camera::OBCameraNodeDriver',
|
||||
parameters=params,
|
||||
extra_arguments=[{'use_intra_process_comms': LaunchConfiguration("use_intra_process_comms")}],
|
||||
)
|
||||
]
|
||||
)
|
||||
]
|
||||
|
||||
return LaunchDescription(
|
||||
args + [
|
||||
OpaqueFunction(function=lambda context: create_node_action(context, args))
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,197 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node,LoadComposableNodes
|
||||
from launch.actions import IncludeLaunchDescription, GroupAction, TimerAction
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.conditions import UnlessCondition, IfCondition
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.substitutions import TextSubstitution
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
|
||||
def load_yaml(file_path):
|
||||
with open(file_path, 'r') as f:
|
||||
return yaml.safe_load(f)
|
||||
|
||||
|
||||
def merge_params(default_params, yaml_params):
|
||||
for key, value in yaml_params.items():
|
||||
if key in default_params:
|
||||
default_params[key] = value
|
||||
return default_params
|
||||
|
||||
|
||||
def convert_value(value):
|
||||
if isinstance(value, str):
|
||||
try:
|
||||
return int(value)
|
||||
except ValueError:
|
||||
pass
|
||||
try:
|
||||
return float(value)
|
||||
except ValueError:
|
||||
pass
|
||||
if value.lower() == 'true':
|
||||
return True
|
||||
elif value.lower() == 'false':
|
||||
return False
|
||||
return value
|
||||
|
||||
|
||||
def load_parameters(context, args):
|
||||
default_params = {arg.name: LaunchConfiguration(arg.name).perform(context) for arg in args}
|
||||
config_file_path = LaunchConfiguration('config_file_path').perform(context)
|
||||
if config_file_path:
|
||||
yaml_params = load_yaml(config_file_path)
|
||||
default_params = merge_params(default_params, yaml_params)
|
||||
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
|
||||
return {
|
||||
key: (value if key in skip_convert else convert_value(value))
|
||||
for key, value in default_params.items()
|
||||
}
|
||||
def generate_launch_description():
|
||||
# Include launch files
|
||||
package_dir = get_package_share_directory("orbbec_camera")
|
||||
launch_file_dir = os.path.join(package_dir, "launch")
|
||||
|
||||
attach_to_shared_component_container_arg = LaunchConfiguration('attach_to_shared_component_container', default=False)
|
||||
component_container_name_arg = LaunchConfiguration('component_container_name', default='shared_orbbec_container')
|
||||
|
||||
shared_orbbec_container = Node(
|
||||
name=component_container_name_arg,
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
output="screen",
|
||||
condition=UnlessCondition(attach_to_shared_component_container_arg),
|
||||
)
|
||||
start_benchmark = LoadComposableNodes(
|
||||
target_container=component_container_name_arg,
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
namespace="start_benchmark",
|
||||
name="start_benchmark",
|
||||
package='orbbec_camera',
|
||||
plugin='orbbec_camera::tools::StartBenchmark',
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
attach_to_shared_component_container_arg = TextSubstitution(text="true")
|
||||
|
||||
args = [
|
||||
DeclareLaunchArgument('uvc_backend', default_value='libuvc'),
|
||||
DeclareLaunchArgument('depth_registration', default_value='false'),
|
||||
DeclareLaunchArgument('color_width', default_value='848'),
|
||||
DeclareLaunchArgument('color_height', default_value='480'),
|
||||
DeclareLaunchArgument('color_fps', default_value='30'),
|
||||
DeclareLaunchArgument('color_format', default_value='MJPG'),
|
||||
DeclareLaunchArgument('enable_color', default_value='true'),
|
||||
DeclareLaunchArgument('depth_width', default_value='848'),
|
||||
DeclareLaunchArgument('depth_height', default_value='480'),
|
||||
DeclareLaunchArgument('depth_fps', default_value='30'),
|
||||
DeclareLaunchArgument('depth_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_depth', default_value='true'),
|
||||
DeclareLaunchArgument('left_ir_width', default_value='848'),
|
||||
DeclareLaunchArgument('left_ir_height', default_value='480'),
|
||||
DeclareLaunchArgument('left_ir_fps', default_value='30'),
|
||||
DeclareLaunchArgument('left_ir_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_left_ir', default_value='true'),
|
||||
DeclareLaunchArgument('right_ir_width', default_value='848'),
|
||||
DeclareLaunchArgument('right_ir_height', default_value='480'),
|
||||
DeclareLaunchArgument('right_ir_fps', default_value='30'),
|
||||
DeclareLaunchArgument('right_ir_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_right_ir', default_value='true'),
|
||||
DeclareLaunchArgument('enable_point_cloud', default_value='false'),
|
||||
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
|
||||
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
|
||||
DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hardware_noise_removal_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_spatial_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'),
|
||||
]
|
||||
launch1_include = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, "gemini_330_series_component.launch.py")
|
||||
),
|
||||
launch_arguments={
|
||||
"camera_name": "camera_01",
|
||||
"usb_port": "2-7",
|
||||
"device_num": "4",
|
||||
"sync_mode": "standalone",
|
||||
"attach_to_shared_component_container": attach_to_shared_component_container_arg,
|
||||
"component_container_name": component_container_name_arg,
|
||||
}.items(),
|
||||
)
|
||||
launch2_include = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, "gemini_330_series_component.launch.py")
|
||||
),
|
||||
launch_arguments={
|
||||
"camera_name": "camera_02",
|
||||
"usb_port": "2-2",
|
||||
"device_num": "4",
|
||||
"sync_mode": "standalone",
|
||||
"attach_to_shared_component_container": attach_to_shared_component_container_arg,
|
||||
"component_container_name": component_container_name_arg,
|
||||
}.items(),
|
||||
)
|
||||
launch3_include = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, "gemini_330_series_component.launch.py")
|
||||
),
|
||||
launch_arguments={
|
||||
"camera_name": "camera_03",
|
||||
"usb_port": "2-1",
|
||||
"device_num": "4",
|
||||
"sync_mode": "standalone",
|
||||
"attach_to_shared_component_container": attach_to_shared_component_container_arg,
|
||||
"component_container_name": component_container_name_arg,
|
||||
}.items(),
|
||||
)
|
||||
launch4_include = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, "gemini_330_series_component.launch.py")
|
||||
),
|
||||
launch_arguments={
|
||||
"camera_name": "camera_04",
|
||||
"usb_port": "2-3",
|
||||
"device_num": "4",
|
||||
"sync_mode": "standalone",
|
||||
"attach_to_shared_component_container": attach_to_shared_component_container_arg,
|
||||
"component_container_name": component_container_name_arg,
|
||||
}.items(),
|
||||
)
|
||||
|
||||
delayed_left_camera = TimerAction(
|
||||
period=2.0,
|
||||
actions=args+[launch1_include],
|
||||
)
|
||||
delayed_right_camera = TimerAction(
|
||||
period=4.0,
|
||||
actions=args+[launch2_include],
|
||||
)
|
||||
delayed_rear_camera = TimerAction(
|
||||
period=6.0,
|
||||
actions=args+[launch3_include],
|
||||
)
|
||||
delayed_front_camera = TimerAction(
|
||||
period=8.0,
|
||||
actions=args+[launch4_include],
|
||||
)
|
||||
ld = LaunchDescription(
|
||||
[
|
||||
shared_orbbec_container,
|
||||
start_benchmark,
|
||||
delayed_left_camera,
|
||||
delayed_right_camera,
|
||||
delayed_rear_camera,
|
||||
delayed_front_camera,
|
||||
]
|
||||
)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,197 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node,LoadComposableNodes
|
||||
from launch.actions import IncludeLaunchDescription, GroupAction, TimerAction
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.conditions import UnlessCondition, IfCondition
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.substitutions import TextSubstitution
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
|
||||
def load_yaml(file_path):
|
||||
with open(file_path, 'r') as f:
|
||||
return yaml.safe_load(f)
|
||||
|
||||
|
||||
def merge_params(default_params, yaml_params):
|
||||
for key, value in yaml_params.items():
|
||||
if key in default_params:
|
||||
default_params[key] = value
|
||||
return default_params
|
||||
|
||||
|
||||
def convert_value(value):
|
||||
if isinstance(value, str):
|
||||
try:
|
||||
return int(value)
|
||||
except ValueError:
|
||||
pass
|
||||
try:
|
||||
return float(value)
|
||||
except ValueError:
|
||||
pass
|
||||
if value.lower() == 'true':
|
||||
return True
|
||||
elif value.lower() == 'false':
|
||||
return False
|
||||
return value
|
||||
|
||||
|
||||
def load_parameters(context, args):
|
||||
default_params = {arg.name: LaunchConfiguration(arg.name).perform(context) for arg in args}
|
||||
config_file_path = LaunchConfiguration('config_file_path').perform(context)
|
||||
if config_file_path:
|
||||
yaml_params = load_yaml(config_file_path)
|
||||
default_params = merge_params(default_params, yaml_params)
|
||||
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
|
||||
return {
|
||||
key: (value if key in skip_convert else convert_value(value))
|
||||
for key, value in default_params.items()
|
||||
}
|
||||
def generate_launch_description():
|
||||
# Include launch files
|
||||
package_dir = get_package_share_directory("orbbec_camera")
|
||||
launch_file_dir = os.path.join(package_dir, "launch")
|
||||
|
||||
attach_to_shared_component_container_arg = LaunchConfiguration('attach_to_shared_component_container', default=False)
|
||||
component_container_name_arg = LaunchConfiguration('component_container_name', default='shared_orbbec_container')
|
||||
|
||||
shared_orbbec_container = Node(
|
||||
name=component_container_name_arg,
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
output="screen",
|
||||
condition=UnlessCondition(attach_to_shared_component_container_arg),
|
||||
)
|
||||
start_benchmark = LoadComposableNodes(
|
||||
target_container=component_container_name_arg,
|
||||
composable_node_descriptions=[
|
||||
ComposableNode(
|
||||
namespace="start_benchmark",
|
||||
name="start_benchmark",
|
||||
package='orbbec_camera',
|
||||
plugin='orbbec_camera::tools::StartBenchmark',
|
||||
)
|
||||
]
|
||||
)
|
||||
|
||||
attach_to_shared_component_container_arg = TextSubstitution(text="true")
|
||||
|
||||
args = [
|
||||
DeclareLaunchArgument('uvc_backend', default_value='libuvc'),
|
||||
DeclareLaunchArgument('depth_registration', default_value='false'),
|
||||
DeclareLaunchArgument('color_width', default_value='848'),
|
||||
DeclareLaunchArgument('color_height', default_value='480'),
|
||||
DeclareLaunchArgument('color_fps', default_value='30'),
|
||||
DeclareLaunchArgument('color_format', default_value='MJPG'),
|
||||
DeclareLaunchArgument('enable_color', default_value='true'),
|
||||
DeclareLaunchArgument('depth_width', default_value='848'),
|
||||
DeclareLaunchArgument('depth_height', default_value='480'),
|
||||
DeclareLaunchArgument('depth_fps', default_value='30'),
|
||||
DeclareLaunchArgument('depth_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_depth', default_value='true'),
|
||||
DeclareLaunchArgument('left_ir_width', default_value='848'),
|
||||
DeclareLaunchArgument('left_ir_height', default_value='480'),
|
||||
DeclareLaunchArgument('left_ir_fps', default_value='30'),
|
||||
DeclareLaunchArgument('left_ir_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_left_ir', default_value='true'),
|
||||
DeclareLaunchArgument('right_ir_width', default_value='848'),
|
||||
DeclareLaunchArgument('right_ir_height', default_value='480'),
|
||||
DeclareLaunchArgument('right_ir_fps', default_value='30'),
|
||||
DeclareLaunchArgument('right_ir_format', default_value='ANY'),
|
||||
DeclareLaunchArgument('enable_right_ir', default_value='true'),
|
||||
DeclareLaunchArgument('enable_point_cloud', default_value='false'),
|
||||
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
|
||||
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
|
||||
DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hardware_noise_removal_filter', default_value='true'),
|
||||
DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_spatial_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
|
||||
DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'),
|
||||
]
|
||||
launch1_include = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, "gemini_330_series_component.launch.py")
|
||||
),
|
||||
launch_arguments={
|
||||
"camera_name": "camera_01",
|
||||
"usb_port": "2-7",
|
||||
"device_num": "4",
|
||||
"sync_mode": "standalone",
|
||||
"attach_to_shared_component_container": attach_to_shared_component_container_arg,
|
||||
"component_container_name": component_container_name_arg,
|
||||
}.items(),
|
||||
)
|
||||
launch2_include = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, "gemini_330_series_component.launch.py")
|
||||
),
|
||||
launch_arguments={
|
||||
"camera_name": "camera_02",
|
||||
"usb_port": "2-2",
|
||||
"device_num": "4",
|
||||
"sync_mode": "standalone",
|
||||
"attach_to_shared_component_container": attach_to_shared_component_container_arg,
|
||||
"component_container_name": component_container_name_arg,
|
||||
}.items(),
|
||||
)
|
||||
launch3_include = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, "gemini_330_series_component.launch.py")
|
||||
),
|
||||
launch_arguments={
|
||||
"camera_name": "camera_03",
|
||||
"usb_port": "2-1",
|
||||
"device_num": "4",
|
||||
"sync_mode": "standalone",
|
||||
"attach_to_shared_component_container": attach_to_shared_component_container_arg,
|
||||
"component_container_name": component_container_name_arg,
|
||||
}.items(),
|
||||
)
|
||||
launch4_include = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, "gemini_330_series_component.launch.py")
|
||||
),
|
||||
launch_arguments={
|
||||
"camera_name": "camera_04",
|
||||
"usb_port": "2-3",
|
||||
"device_num": "4",
|
||||
"sync_mode": "standalone",
|
||||
"attach_to_shared_component_container": attach_to_shared_component_container_arg,
|
||||
"component_container_name": component_container_name_arg,
|
||||
}.items(),
|
||||
)
|
||||
|
||||
delayed_left_camera = TimerAction(
|
||||
period=2.0,
|
||||
actions=args+[launch1_include],
|
||||
)
|
||||
delayed_right_camera = TimerAction(
|
||||
period=4.0,
|
||||
actions=args+[launch2_include],
|
||||
)
|
||||
delayed_rear_camera = TimerAction(
|
||||
period=6.0,
|
||||
actions=args+[launch3_include],
|
||||
)
|
||||
delayed_front_camera = TimerAction(
|
||||
period=8.0,
|
||||
actions=args+[launch4_include],
|
||||
)
|
||||
ld = LaunchDescription(
|
||||
[
|
||||
shared_orbbec_container,
|
||||
start_benchmark,
|
||||
delayed_left_camera,
|
||||
delayed_right_camera,
|
||||
delayed_rear_camera,
|
||||
delayed_front_camera,
|
||||
]
|
||||
)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,324 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_components/register_node_macro.hpp>
|
||||
#include <orbbec_camera/ob_camera_node_driver.h>
|
||||
#include <orbbec_camera/utils.h>
|
||||
#include "orbbec_camera_msgs/msg/metadata.hpp"
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <filesystem>
|
||||
#include <cpuid.h>
|
||||
|
||||
namespace orbbec_camera {
|
||||
namespace tools {
|
||||
|
||||
class ObBenchmark : public rclcpp::Node {
|
||||
public:
|
||||
ObBenchmark() : Node("ObBenchmark") {
|
||||
params_init();
|
||||
stat_process();
|
||||
function_ = this->create_wall_timer(std::chrono::seconds(test_cycle_),
|
||||
std::bind(&ObBenchmark::functionCallback, this));
|
||||
config_ = this->create_wall_timer(std::chrono::seconds(switch_cycle_),
|
||||
std::bind(&ObBenchmark::configCallback, this));
|
||||
}
|
||||
~ObBenchmark() { kill_process(process_name_); }
|
||||
|
||||
private:
|
||||
rclcpp::TimerBase::SharedPtr function_;
|
||||
rclcpp::TimerBase::SharedPtr config_;
|
||||
|
||||
std::vector<float> cpu_usage_;
|
||||
std::vector<float> memory_usage_;
|
||||
std::vector<std::string> current_time_;
|
||||
|
||||
int usage_count_ = 0;
|
||||
int launch_count_ = 0;
|
||||
long prevIdle = 0;
|
||||
long prevTotal = 0;
|
||||
std::string process_pid_;
|
||||
|
||||
std::string process_name_;
|
||||
int switch_cycle_;
|
||||
int test_cycle_;
|
||||
int skip_number_;
|
||||
|
||||
std::mutex image_mutex_;
|
||||
|
||||
void params_init() {
|
||||
std::ifstream file(
|
||||
"install/orbbec_camera/share/orbbec_camera/config/tools/startbenchmark/"
|
||||
"start_benchmark_params.json");
|
||||
if (!file.is_open()) {
|
||||
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to open JSON file.");
|
||||
return;
|
||||
}
|
||||
nlohmann::json json_data;
|
||||
file >> json_data;
|
||||
process_name_ = json_data["start_benchmark_params"]["process_name"].get<std::string>();
|
||||
switch_cycle_ = json_data["start_benchmark_params"]["switch_cycle"].get<int>();
|
||||
test_cycle_ = json_data["start_benchmark_params"]["test_cycle"].get<int>();
|
||||
skip_number_ = json_data["start_benchmark_params"]["skip_number"].get<int>();
|
||||
}
|
||||
std::string exec_command(const std::string& command) {
|
||||
std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
|
||||
if (!pipe) {
|
||||
std::cerr << "Failed to run command" << std::endl;
|
||||
return "";
|
||||
}
|
||||
|
||||
char buffer[128];
|
||||
std::string result = "";
|
||||
while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) {
|
||||
result += buffer;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
float get_ps_cpu_usage_by_process(const std::string& process_name) {
|
||||
std::string command = "ps aux | grep '" + process_name + "' | grep -v grep";
|
||||
std::string result = exec_command(command);
|
||||
std::istringstream stream(result);
|
||||
std::string line;
|
||||
float total_cpu_usage = 0.0;
|
||||
|
||||
while (std::getline(stream, line)) {
|
||||
std::istringstream line_stream(line);
|
||||
std::string user, pid, cpu, mem, command;
|
||||
line_stream >> user >> pid >> cpu >> mem;
|
||||
|
||||
if (cpu.empty() || line.find("grep") != std::string::npos) {
|
||||
continue;
|
||||
}
|
||||
|
||||
try {
|
||||
process_pid_ = pid;
|
||||
float cpu_usage = std::stof(cpu);
|
||||
total_cpu_usage += cpu_usage;
|
||||
} catch (const std::invalid_argument& e) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
return total_cpu_usage;
|
||||
}
|
||||
|
||||
float get_top_cpu_usage_by_process() {
|
||||
std::string command =
|
||||
"top -bn1 -p " + process_pid_ + " | grep '" + process_pid_ + "' | awk '{print $9}'";
|
||||
std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
|
||||
if (!pipe) {
|
||||
std::cerr << "Failed to run command" << std::endl;
|
||||
return -1.0f;
|
||||
}
|
||||
char buffer[128];
|
||||
float total_cpu_usage = 0.0f;
|
||||
bool found_process = false;
|
||||
while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) {
|
||||
std::stringstream ss(buffer);
|
||||
float cpu_usage = 0.0f;
|
||||
ss >> cpu_usage;
|
||||
if (ss) {
|
||||
total_cpu_usage += cpu_usage;
|
||||
found_process = true;
|
||||
}
|
||||
}
|
||||
if (!found_process) {
|
||||
std::cerr << "No matching processes found for PID: " << process_pid_ << std::endl;
|
||||
return -1.0f;
|
||||
}
|
||||
return total_cpu_usage;
|
||||
}
|
||||
float get_memory_usage_by_process() {
|
||||
std::string command =
|
||||
"top -b -n 1 -p " + process_pid_ + " | grep '" + process_pid_ + "' | awk '{print $6}'";
|
||||
std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
|
||||
if (!pipe) {
|
||||
std::cerr << "Failed to run command" << std::endl;
|
||||
return -1.0f;
|
||||
}
|
||||
char buffer[128];
|
||||
std::string result = "";
|
||||
|
||||
while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) {
|
||||
result += buffer;
|
||||
}
|
||||
std::stringstream ss(result);
|
||||
long memory_usage_kb;
|
||||
ss >> memory_usage_kb;
|
||||
float memory_usage_mb = static_cast<float>(memory_usage_kb) / 1024.0f;
|
||||
std::stringstream formatted_result;
|
||||
formatted_result << std::fixed << std::setprecision(1) << memory_usage_mb;
|
||||
return std::stof(formatted_result.str());
|
||||
}
|
||||
|
||||
std::string get_cpu() {
|
||||
unsigned int CPUInfo[4] = {0, 0, 0, 0};
|
||||
char CPUBrandString[0x40] = {0};
|
||||
|
||||
__cpuid(0x80000000, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]);
|
||||
|
||||
for (unsigned int i = 0x80000002; i <= 0x80000004; ++i) {
|
||||
__cpuid(i, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]);
|
||||
|
||||
if (i == 0x80000002) {
|
||||
memcpy(CPUBrandString, CPUInfo, sizeof(CPUInfo));
|
||||
} else if (i == 0x80000003) {
|
||||
memcpy(CPUBrandString + 16, CPUInfo, sizeof(CPUInfo));
|
||||
} else if (i == 0x80000004) {
|
||||
memcpy(CPUBrandString + 32, CPUInfo, sizeof(CPUInfo));
|
||||
}
|
||||
}
|
||||
|
||||
char* ptr = CPUBrandString;
|
||||
while (*ptr == ' ') ptr++;
|
||||
return std::string(ptr);
|
||||
}
|
||||
std::string getCurrentTimes() {
|
||||
auto now = std::chrono::system_clock::now();
|
||||
auto now_time_t = std::chrono::system_clock::to_time_t(now);
|
||||
std::tm tm = *std::localtime(&now_time_t);
|
||||
std::ostringstream date_stream;
|
||||
date_stream << std::put_time(&tm, "%H:%M:%S");
|
||||
std::string date_str = date_stream.str();
|
||||
return date_str;
|
||||
}
|
||||
void ensure_directory_exists(const std::string& path) {
|
||||
size_t found = path.find_last_of("/\\");
|
||||
std::string dir_path = path.substr(0, found);
|
||||
|
||||
struct stat info;
|
||||
if (stat(dir_path.c_str(), &info) != 0) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
||||
"Directory does not exist, creating it: " << dir_path);
|
||||
if (mkdir(dir_path.c_str(), 0777) != 0) {
|
||||
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to create directory: " << dir_path.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
void save_to_csv(const std::string& filename) {
|
||||
ensure_directory_exists(filename);
|
||||
std::ofstream file;
|
||||
file.open(filename, std::ios_base::app);
|
||||
if (file.is_open()) {
|
||||
file << "Time,CPU,Memory(MB)" << "\n";
|
||||
for (int i = skip_number_; i < usage_count_ - 1; i++) {
|
||||
file << current_time_[i] << "," << cpu_usage_[i] << "%" << "," << memory_usage_[i] << "\n";
|
||||
}
|
||||
float cpu_average =
|
||||
std::accumulate(cpu_usage_.begin() + skip_number_, cpu_usage_.begin() + usage_count_ - 1, 0.0f) /
|
||||
(usage_count_ - (skip_number_+1));
|
||||
float memory_average = std::accumulate(memory_usage_.begin() + skip_number_,
|
||||
memory_usage_.begin() + usage_count_ - 1, 0.0f) /
|
||||
(usage_count_ - (skip_number_+1));
|
||||
file << "Average: ," << cpu_average << "%, " << memory_average << "\n";
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
||||
"Average: " << cpu_average << "%" << memory_average << "MB");
|
||||
file.close();
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to open file for writing.");
|
||||
}
|
||||
cpu_usage_.clear();
|
||||
memory_usage_.clear();
|
||||
current_time_.clear();
|
||||
usage_count_ = 0;
|
||||
++launch_count_;
|
||||
}
|
||||
void kill_process(const std::string& process_name) {
|
||||
std::string command = "ps aux | grep " + process_name + " | grep -v grep | awk '{print $2}'";
|
||||
|
||||
std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
|
||||
if (!pipe) {
|
||||
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to run command to find PID.");
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<int> pids;
|
||||
char path[1035];
|
||||
while (fgets(path, sizeof(path), pipe.get()) != nullptr) {
|
||||
int pid = std::stoi(path);
|
||||
pids.push_back(pid);
|
||||
std::cout << "----------------------------------------------------------" << std::endl;
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"), "Found PID: " << pid);
|
||||
}
|
||||
|
||||
if (pids.empty()) {
|
||||
RCLCPP_WARN(this->get_logger(), "No matching processes found.");
|
||||
return;
|
||||
}
|
||||
|
||||
for (int pid : pids) {
|
||||
if (kill(pid, SIGINT) == 0) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
||||
"Successfully sent SIGINT (Ctrl+C) to process with PID " << pid);
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(this->get_logger(),
|
||||
"Failed to send SIGINT to process with PID " << pid);
|
||||
}
|
||||
}
|
||||
}
|
||||
void stat_process() {
|
||||
const char* ros2_launch = "/opt/ros/humble/bin/ros2";
|
||||
std::string start_launch = "ob_benchmark_" + std::to_string(launch_count_) + ".launch.py";
|
||||
std::vector<const char*> args = {ros2_launch, "launch", "orbbec_camera", start_launch.c_str(),
|
||||
nullptr};
|
||||
|
||||
pid_t pid = fork();
|
||||
|
||||
if (pid == 0) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
||||
"Child process: launching ros2 launch.");
|
||||
if (execvp(ros2_launch, (char* const*)args.data()) == -1) {
|
||||
RCLCPP_ERROR_STREAM(this->get_logger(),
|
||||
"Failed to execute ros2 launch: " << strerror(errno));
|
||||
exit(1);
|
||||
}
|
||||
} else if (pid > 0) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
||||
"Parent process: continuing execution.");
|
||||
} else {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"), "Fork failed!");
|
||||
}
|
||||
}
|
||||
|
||||
void functionCallback() {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
// RCLCPP_INFO_STREAM(
|
||||
// rclcpp::get_logger("ObBenchmark"),
|
||||
// "get_ps_cpu_usage_by_process: " << get_ps_cpu_usage_by_process(process_name_));
|
||||
// RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
||||
// "get_top_cpu_usage_by_process: " << get_top_cpu_usage_by_process());
|
||||
float cpu_usage =
|
||||
get_ps_cpu_usage_by_process(process_name_) * 0.4 + get_top_cpu_usage_by_process() * 0.6;
|
||||
float memory_usage = get_memory_usage_by_process();
|
||||
cpu_usage_.push_back(cpu_usage);
|
||||
memory_usage_.push_back(memory_usage);
|
||||
current_time_.push_back(getCurrentTimes());
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
||||
"CurrentTimes: " << current_time_[usage_count_]);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"), "CPU Usage of "
|
||||
<< process_name_ << ":"
|
||||
<< cpu_usage_[usage_count_] << "%");
|
||||
RCLCPP_INFO_STREAM(
|
||||
rclcpp::get_logger("ObBenchmark"),
|
||||
"Memory Usage of " << process_name_ << ":" << memory_usage_[usage_count_] << "MB");
|
||||
usage_count_++;
|
||||
}
|
||||
void configCallback() {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
std::string csv = std::string("ob_benchmark/") + std::to_string(launch_count_) + ".csv";
|
||||
|
||||
save_to_csv(csv);
|
||||
kill_process(process_name_);
|
||||
stat_process();
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace tools
|
||||
} // namespace orbbec_camera
|
||||
int main(int argc, char** argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<orbbec_camera::tools::ObBenchmark>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,149 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_components/register_node_macro.hpp>
|
||||
#include <orbbec_camera/ob_camera_node_driver.h>
|
||||
#include <orbbec_camera/utils.h>
|
||||
#include "orbbec_camera_msgs/msg/metadata.hpp"
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <filesystem>
|
||||
#include <cpuid.h>
|
||||
|
||||
namespace orbbec_camera {
|
||||
namespace tools {
|
||||
|
||||
class StartBenchmark : public rclcpp::Node {
|
||||
public:
|
||||
explicit StartBenchmark(const rclcpp::NodeOptions& options) : Node("StartBenchmark", options) {
|
||||
params_init();
|
||||
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
|
||||
for (size_t i = 0; i < camera_name_.size(); ++i) {
|
||||
color_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
|
||||
color_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
|
||||
this->color_Callback(msg, i);
|
||||
}));
|
||||
depth_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
|
||||
depth_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
|
||||
this->depth_Callback(msg, i);
|
||||
}));
|
||||
left_ir_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
|
||||
left_ir_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
|
||||
this->left_ir_Callback(msg, i);
|
||||
}));
|
||||
right_ir_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
|
||||
right_ir_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
|
||||
this->right_ir_Callback(msg, i);
|
||||
}));
|
||||
depth_point_cloud_subs_.push_back(this->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
depth_point_cloud_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
|
||||
this->depth_point_cloud_Callback(msg, i);
|
||||
}));
|
||||
color_point_cloud_subs_.push_back(this->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
color_point_cloud_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
|
||||
this->color_point_cloud_Callback(msg, i);
|
||||
}));
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), color_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), depth_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), left_ir_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), right_ir_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
depth_point_cloud_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
color_point_cloud_topics_[i] << " is subed ");
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> depth_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> left_ir_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> right_ir_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
|
||||
depth_point_cloud_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
|
||||
color_point_cloud_subs_;
|
||||
|
||||
|
||||
std::vector<std::string> camera_name_;
|
||||
std::vector<std::string> color_topics_;
|
||||
std::vector<std::string> depth_topics_;
|
||||
std::vector<std::string> left_ir_topics_;
|
||||
std::vector<std::string> right_ir_topics_;
|
||||
std::vector<std::string> depth_point_cloud_topics_;
|
||||
std::vector<std::string> color_point_cloud_topics_;
|
||||
|
||||
std::mutex image_mutex_;
|
||||
|
||||
void params_init() {
|
||||
std::ifstream file(
|
||||
"install/orbbec_camera/share/orbbec_camera/config/tools/startbenchmark/"
|
||||
"start_benchmark_params.json");
|
||||
if (!file.is_open()) {
|
||||
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to open JSON file.");
|
||||
return;
|
||||
}
|
||||
nlohmann::json json_data;
|
||||
file >> json_data;
|
||||
camera_name_ =
|
||||
json_data["start_benchmark_params"]["camera_name"].get<std::vector<std::string>>();
|
||||
color_topics_.resize(camera_name_.size());
|
||||
depth_topics_.resize(camera_name_.size());
|
||||
left_ir_topics_.resize(camera_name_.size());
|
||||
right_ir_topics_.resize(camera_name_.size());
|
||||
depth_point_cloud_topics_.resize(camera_name_.size());
|
||||
color_point_cloud_topics_.resize(camera_name_.size());
|
||||
for (size_t i = 0; i < camera_name_.size(); ++i) {
|
||||
color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
|
||||
depth_topics_[i] = "/" + camera_name_[i] + "/depth/image_raw";
|
||||
left_ir_topics_[i] = "/" + camera_name_[i] + "/left_ir/image_raw";
|
||||
right_ir_topics_[i] = "/" + camera_name_[i] + "/right_ir/image_raw";
|
||||
depth_point_cloud_topics_[i] = "/" + camera_name_[i] + "/depth/points";
|
||||
color_point_cloud_topics_[i] = "/" + camera_name_[i] + "/depth_registered/points";
|
||||
}
|
||||
}
|
||||
|
||||
void color_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->step << "color is subed " << index << "is subed");
|
||||
}
|
||||
void depth_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->step << "depth is subed " << index << "is subed");
|
||||
}
|
||||
void left_ir_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->step << "left_ir is subed " << index << "is subed");
|
||||
}
|
||||
void right_ir_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->step << "right_ir is subed " << index << "is subed");
|
||||
}
|
||||
void depth_point_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg,
|
||||
size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->point_step << "depth_point_cloud is subed " << index << "is subed");
|
||||
}
|
||||
void color_point_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg,
|
||||
size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->point_step << "color_point_cloud is subed " << index << "is subed");
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace tools
|
||||
} // namespace orbbec_camera
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::tools::StartBenchmark)
|
||||
Reference in New Issue
Block a user