diff --git a/orbbec_camera/launch/multi_camera.launch.py b/orbbec_camera/launch/multi_camera.launch.py index fd86a54e..6c68f936 100644 --- a/orbbec_camera/launch/multi_camera.launch.py +++ b/orbbec_camera/launch/multi_camera.launch.py @@ -1,21 +1,12 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction, ExecuteProcess -from launch.substitutions import LaunchConfiguration from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os def generate_launch_description(): - # Declare arguments - camera_name = DeclareLaunchArgument('camera_name', default_value='camera') - d_sensor = DeclareLaunchArgument('3d_sensor', default_value='gemini2') - camera1_prefix = DeclareLaunchArgument('camera1_prefix', default_value='01') - camera2_prefix = DeclareLaunchArgument('camera2_prefix', default_value='02') - camera1_usb_port = DeclareLaunchArgument('camera1_usb_port', default_value='2-3.3') - camera2_usb_port = DeclareLaunchArgument('camera2_usb_port', default_value='1-4.4') - device_num = DeclareLaunchArgument('device_num', default_value='2') - # Node configuration cleanup_node = Node( package='orbbec_camera', @@ -25,46 +16,37 @@ def generate_launch_description(): ) # Include launch files - launch_file_dir = FindPackageShare('orbbec_camera').find('orbbec_camera') + '/launch' + package_dir = get_package_share_directory('orbbec_camera') + launch_file_dir = os.path.join(package_dir, 'launch') launch1_include = IncludeLaunchDescription( - PythonLaunchDescriptionSource(launch_file_dir + '/' + (LaunchConfiguration('3d_sensor') + '.launch.py')), + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'gemini2.launch.py') + ), launch_arguments={ - 'camera_name': 'camera_' + LaunchConfiguration('camera1_prefix'), - 'usb_port': LaunchConfiguration('camera1_usb_port'), - 'device_num': LaunchConfiguration('device_num') + 'camera_name': 'camera_01', + 'usb_port': '6-2.4.4.2', + 'device_num': '2' }.items() ) launch2_include = IncludeLaunchDescription( - PythonLaunchDescriptionSource(launch_file_dir + '/' + (LaunchConfiguration('3d_sensor') + '.launch.py')), + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'gemini2.launch.py') + ), launch_arguments={ - 'camera_name': 'camera_' + LaunchConfiguration('camera2_prefix'), - 'usb_port': LaunchConfiguration('camera2_usb_port'), - 'device_num': LaunchConfiguration('device_num') + 'camera_name': 'camera_02', + 'usb_port': '6-2.4.1', + 'device_num': '2' }.items() ) - # Static TF publisher - tf_publisher = Node( - package='tf2_ros', - executable='static_transform_publisher', - name='camera_tf', - arguments=['0', '0', '0', '0', '0', '0', 'camera01_link', 'camera02_link'] - ) + # If you need more cameras, just add more launch_include here, and change the usb_port and device_num # Launch description ld = LaunchDescription([ - camera_name, - d_sensor, - camera1_prefix, - camera2_prefix, - camera1_usb_port, - camera2_usb_port, - device_num, cleanup_node, GroupAction([launch1_include]), GroupAction([launch2_include]), - tf_publisher ]) return ld diff --git a/orbbec_camera/src/list_devices_node.cpp b/orbbec_camera/src/list_devices_node.cpp index f0c5e330..b033fd51 100644 --- a/orbbec_camera/src/list_devices_node.cpp +++ b/orbbec_camera/src/list_devices_node.cpp @@ -4,17 +4,25 @@ #include int main() { - auto context = std::make_unique(); - context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE); - auto list = context->queryDeviceList(); - for (size_t i = 0; i < list->deviceCount(); i++) { - auto device = list->getDevice(i); - auto device_info = device->getDeviceInfo(); - std::string serial = device_info->serialNumber(); - std::string uid = device_info->uid(); - auto usb_port = orbbec_camera::parseUsbPort(uid); - RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "serial: " << serial); - RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "usb port: " << usb_port); + try { + auto context = std::make_unique(); + context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE); + auto list = context->queryDeviceList(); + for (size_t i = 0; i < list->deviceCount(); i++) { + auto device = list->getDevice(i); + auto device_info = device->getDeviceInfo(); + std::string serial = device_info->serialNumber(); + std::string uid = device_info->uid(); + auto usb_port = orbbec_camera::parseUsbPort(uid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "serial: " << serial); + RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "usb port: " << usb_port); + } + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("list_device_node"), e.what()); + } catch (ob::Error &e) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("list_device_node"), e.getMessage()); + } catch (...) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("list_device_node"), "unknown error"); } return 0; }