diff --git a/zed_wrapper/config/common_mono.yaml b/zed_wrapper/config/common_mono.yaml index 67c88495..719f39d7 100644 --- a/zed_wrapper/config/common_mono.yaml +++ b/zed_wrapper/config/common_mono.yaml @@ -13,7 +13,7 @@ video: saturation: 4 # [DYNAMIC] - sharpness: 4 # [DYNAMIC] + sharpness: 8 # [DYNAMIC] gamma: 8 # [DYNAMIC] auto_whitebalance: true # [DYNAMIC] whitebalance_temperature: 42 # [DYNAMIC] - [28,65] x100 - works only if `auto_whitebalance` is false diff --git a/zed_wrapper/config/common_stereo.yaml b/zed_wrapper/config/common_stereo.yaml index 57fdb6cb..264e367b 100644 --- a/zed_wrapper/config/common_stereo.yaml +++ b/zed_wrapper/config/common_stereo.yaml @@ -25,9 +25,9 @@ camera_flip: false self_calib: true # Enable the self-calibration process at camera opening. See https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html#affeaa06cfc1d849e311e484ceb8edcc5 serial_number: 0 # overwritten by launch file - 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_resolution: 'NATIVE' # 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 + pub_frame_rate: 10.0 # frequency of publishing of visual images and depth images enable_image_validity_check: 1 # [SDK5 required] Sets the image validity check. If set to 1, the SDK will check if the frames are valid before processing. gpu_id: -1 optional_opencv_calibration_file: '' # Optional path where the ZED SDK can find a file containing the calibration information of the camera computed by OpenCV. Read the ZED SDK documentation for more information: https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html#a9eab2753374ef3baec1d31960859ba19 @@ -37,7 +37,7 @@ video: saturation: 4 # [DYNAMIC] - sharpness: 4 # [DYNAMIC] + sharpness: 8 # [DYNAMIC] gamma: 8 # [DYNAMIC] auto_exposure_gain: true # [DYNAMIC] exposure: 80 # [DYNAMIC] @@ -50,7 +50,7 @@ sensors: publish_imu_tf: false # [overwritten by launch file options] enable/disable the IMU TF broadcasting sensors_image_sync: false # Synchronize Sensors messages with latest published video/depth message - sensors_pub_rate: 100. # frequency of publishing of sensors data. MAX: 400. - MIN: grab rate + sensors_pub_rate: 15. # frequency of publishing of sensors data. MAX: 400. - MIN: grab rate region_of_interest: automatic_roi: false # Enable the automatic ROI generation to automatically detect part of the robot in the FoV and remove them from the processing. Note: if enabled the value of `manual_polygon` is ignored @@ -68,9 +68,9 @@ depth: depth_mode: 'NEURAL_LIGHT' # 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, ...) - depth_stabilization: 30 # Forces positional tracking to start if major than 0 - Range: [0,100] + depth_stabilization: 0 # Forces positional tracking to start if major than 0 - Range: [0,100] openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] - point_cloud_freq: 15.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `pub_frame_rate` value) + point_cloud_freq: 2.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_confidence: 95 # [DYNAMIC] depth_texture_conf: 100 # [DYNAMIC] @@ -79,7 +79,7 @@ # 'zedx.yaml', 'zedxmini.yaml', 'virtual.yaml' files pos_tracking: - pos_tracking_enabled: true # True to enable positional tracking from start + pos_tracking_enabled: false # True to enable positional tracking from start pos_tracking_mode: 'GEN_1' # Matches the ZED SDK setting: 'GEN_1', 'GEN_2', 'GEN_3' imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. publish_tf: true # [overwritten by launch file options] publish `odom -> camera_link` TF diff --git a/zed_wrapper/config/zed_launch_config.yaml b/zed_wrapper/config/zed_launch_config.yaml new file mode 100644 index 00000000..fc8e90a4 --- /dev/null +++ b/zed_wrapper/config/zed_launch_config.yaml @@ -0,0 +1,15 @@ +# ZED Camera Launch Configuration +# This file determines which launch configuration to use + +# Mode selection: "single" or "multi" +mode: "multi" + +# Single camera configuration (used when mode: "single") +single_camera: + camera_model: "zedm" # Required: zed, zedm, zed2, zed2i, zedx, zedxm + +# Multi camera configuration (used when mode: "multi") +multi_camera: + cam_names: ["front_zed", "back_zed"] + cam_models: ["zedm", "zedm"] + cam_serials: [17296654, 15759272] diff --git a/zed_wrapper/config/zedm.yaml b/zed_wrapper/config/zedm.yaml index e1aa71f1..225c7318 100644 --- a/zed_wrapper/config/zedm.yaml +++ b/zed_wrapper/config/zedm.yaml @@ -7,14 +7,14 @@ camera_model: 'zedm' camera_name: 'zedm' # overwritten by launch file grab_resolution: 'HD2K' # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' - grab_frame_rate: 30 # ZED SDK internal grabbing rate + grab_frame_rate: 15 # ZED SDK internal grabbing rate video: brightness: 4 # [DYNAMIC] Image brightness. Range: 0-8 - contrast: 4 # [DYNAMIC] Image contrast. Range: 0-8 + contrast: 8 # [DYNAMIC] Image contrast. Range: 0-8 hue: 0 # [DYNAMIC] Image hue. Range: 0 to 11 depth: min_depth: 0.1 # Min: 0.1, Max: 3.0 - max_depth: 2.0 # Max: 20.0 + max_depth: 5.0 # Max: 20.0 diff --git a/zed_wrapper/launch/zed_auto.launch.py b/zed_wrapper/launch/zed_auto.launch.py new file mode 100644 index 00000000..3eb7728f --- /dev/null +++ b/zed_wrapper/launch/zed_auto.launch.py @@ -0,0 +1,153 @@ +# Copyright 2024 Stereolabs +# +# Licensed under the Apache License, Version 2.0 (the 'License'); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an 'AS IS' BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + OpaqueFunction, + IncludeLaunchDescription, + LogInfo +) +from launch.substitutions import ( + LaunchConfiguration, + TextSubstitution +) + +def load_config_file(config_path): + """Load and parse the YAML configuration file""" + try: + with open(config_path, 'r') as file: + config = yaml.safe_load(file) + return config + except Exception as e: + print(f"Error loading config file {config_path}: {e}") + return None + +def launch_setup(context, *args, **kwargs): + """Setup function that determines which launch file to use based on config""" + + actions = [] + + # Automatically find the config file + wrapper_dir = get_package_share_directory('zed_wrapper') + config_path = os.path.join(wrapper_dir, 'config', 'zed_launch_config.yaml') + + actions.append(LogInfo(msg=TextSubstitution(text=f"Loading config from: {config_path}"))) + + # Load configuration + config = load_config_file(config_path) + if config is None: + return [LogInfo(msg=TextSubstitution(text="Failed to load configuration file. Using default single camera mode."))] + + mode = config.get('mode', 'single').lower() + + # Log the selected mode + actions.append(LogInfo(msg=TextSubstitution(text=f"ZED Launch Mode: {mode}"))) + + # wrapper_dir already defined above + + if mode == 'single': + # Single camera configuration + single_config = config.get('single_camera', {}) + + actions.append(LogInfo(msg=TextSubstitution(text="Launching single ZED camera configuration"))) + + # Prepare launch arguments for single camera - only pass specified parameters + launch_args = {} + + # Required parameter + if 'camera_model' in single_config: + launch_args['camera_model'] = single_config['camera_model'] + else: + actions.append(LogInfo(msg=TextSubstitution(text="ERROR: camera_model is required in single_camera config"))) + return actions + + # Optional parameters - only add if specified in config + if 'camera_name' in single_config: + launch_args['camera_name'] = single_config['camera_name'] + if 'serial_number' in single_config: + launch_args['serial_number'] = str(single_config['serial_number']) + if 'camera_id' in single_config: + launch_args['camera_id'] = str(single_config['camera_id']) + if 'namespace' in single_config: + launch_args['namespace'] = single_config['namespace'] + + # Include single camera launch file + single_camera_launch = IncludeLaunchDescription( + launch_description_source=PythonLaunchDescriptionSource([ + wrapper_dir, '/launch/zed_camera.launch.py' + ]), + launch_arguments=launch_args.items() + ) + actions.append(single_camera_launch) + + elif mode == 'multi': + # Multi camera configuration + multi_config = config.get('multi_camera', {}) + + actions.append(LogInfo(msg=TextSubstitution(text="Launching multi ZED camera configuration"))) + + # Required parameters + if 'cam_names' not in multi_config or 'cam_models' not in multi_config: + actions.append(LogInfo(msg=TextSubstitution(text="ERROR: cam_names and cam_models are required in multi_camera config"))) + return actions + + cam_names = multi_config['cam_names'] + cam_models = multi_config['cam_models'] + + # Optional parameters + cam_serials = multi_config.get('cam_serials', []) + cam_ids = multi_config.get('cam_ids', []) + + # Format arrays as strings + cam_names_str = '[' + ','.join(cam_names) + ']' + cam_models_str = '[' + ','.join(cam_models) + ']' + cam_serials_str = '[' + ','.join(map(str, cam_serials)) + ']' if cam_serials else '[]' + cam_ids_str = '[' + ','.join(map(str, cam_ids)) + ']' if cam_ids else '[]' + + # Prepare launch arguments for multi camera - only required ones + launch_args = { + 'cam_names': cam_names_str, + 'cam_models': cam_models_str, + 'cam_serials': cam_serials_str, + 'cam_ids': cam_ids_str + } + + # Include multi camera launch file + multi_camera_launch = IncludeLaunchDescription( + launch_description_source=PythonLaunchDescriptionSource([ + wrapper_dir, '/launch/zed_multi_camera.launch.py' + ]), + launch_arguments=launch_args.items() + ) + actions.append(multi_camera_launch) + + else: + actions.append(LogInfo(msg=TextSubstitution( + text=f"Unknown mode '{mode}'. Please set mode to 'single' or 'multi' in the config file."))) + + return actions + +def generate_launch_description(): + """Generate the launch description""" + + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/zed_wrapper/launch/zed_camera.launch.py b/zed_wrapper/launch/zed_camera.launch.py index e10ea0b6..d9eafd3b 100644 --- a/zed_wrapper/launch/zed_camera.launch.py +++ b/zed_wrapper/launch/zed_camera.launch.py @@ -267,7 +267,7 @@ def launch_setup(context, *args, **kwargs): ] if( ros_params_override_path_val != ''): - node_parameters.append(ros_params_override_path) + node_parameters.append(ros_params_override) node_parameters.append( # Launch arguments must override the YAML files values @@ -291,6 +291,13 @@ def launch_setup(context, *args, **kwargs): } ) + # Ensure only raw image_transport plugins are used at startup to avoid + # intermittent plugin loading issues for compressed publishers + node_parameters.append({ + 'image_transport/publisher_plugins': ['raw'], + 'image_transport/subscriber_plugins': ['raw'] + }) + # ZED Wrapper component if( camera_model_val=='zed' or @@ -384,7 +391,7 @@ def generate_launch_description(): choices=['true', 'false']), DeclareLaunchArgument( 'publish_tf', - default_value='true', + default_value='false', description='Enable publication of the `odom -> camera_link` TF.', choices=['true', 'false']), DeclareLaunchArgument( diff --git a/zed_wrapper/launch/zed_multi_camera.launch.py b/zed_wrapper/launch/zed_multi_camera.launch.py new file mode 100644 index 00000000..e5f51b8c --- /dev/null +++ b/zed_wrapper/launch/zed_multi_camera.launch.py @@ -0,0 +1,189 @@ +# Copyright 2024 Stereolabs +# +# Licensed under the Apache License, Version 2.0 (the 'License'); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an 'AS IS' BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + OpaqueFunction, + IncludeLaunchDescription, + LogInfo, + TimerAction, + SetEnvironmentVariable, + RegisterEventHandler +) +from launch.event_handlers import OnProcessStart +from launch.substitutions import ( + LaunchConfiguration, + TextSubstitution +) +from launch_ros.actions import ( + ComposableNodeContainer +) + +def parse_array_param(param): + str = param.replace('[', '') + str = str.replace(']', '') + str = str.replace(' ', '') + arr = str.split(',') + + return arr + +def launch_setup(context, *args, **kwargs): + + # List of actions to be launched + actions = [] + + # Force a safe default image transport at startup to avoid plugin race/loading issues + actions.append(SetEnvironmentVariable(name='ROS_IMAGE_TRANSPORT', value='raw')) + + namespace_val = 'zed_multi' + + names = LaunchConfiguration('cam_names') + models = LaunchConfiguration('cam_models') + serials = LaunchConfiguration('cam_serials') + ids = LaunchConfiguration('cam_ids') + + names_arr = parse_array_param(names.perform(context)) + models_arr = parse_array_param(models.perform(context)) + serials_arr = parse_array_param(serials.perform(context)) + ids_arr = parse_array_param(ids.perform(context)) + + num_cams = len(names_arr) + + if (num_cams != len(models_arr)): + return [ + LogInfo(msg=TextSubstitution( + text='The `cam_models` array argument must match the size of the `cam_names` array argument.')) + ] + + if ((num_cams != len(serials_arr)) and (num_cams != len(ids_arr))): + return [ + LogInfo(msg=TextSubstitution( + text='The `cam_serials` or `cam_ids` array argument must match the size of the `cam_names` array argument.')) + ] + + # Do not create a single shared container; instead, create one per camera for isolation + # container_name is used as a base for per-camera containers + container_name = 'zed_multi_container' + distro = os.environ['ROS_DISTRO'] + if distro == 'foxy': + container_exec='component_container' + else: + container_exec='component_container_isolated' + + info = '* Starting Composable node container: /' + namespace_val + '/' + container_name + actions.append(LogInfo(msg=TextSubstitution(text=info))) + + # Set the first camera idx + cam_idx = 0 + + for name in names_arr: + model = models_arr[cam_idx] + if len(serials_arr) == num_cams: + serial = serials_arr[cam_idx] + else: + serial = '0' + + if len(ids_arr) == num_cams: + id = ids_arr[cam_idx] + else: + id = '-1' + + info = '* Starting a ZED ROS2 node for camera ' + name + \ + ' (' + model + if(serial != '0'): + info += ', serial: ' + serial + elif( id!= '-1'): + info += ', id: ' + id + info += ')' + + actions.append(LogInfo(msg=TextSubstitution(text=info))) + + # TF publishing is permanently disabled + publish_tf = 'false' + + # Per-camera container for better isolation of pluginlib loading + per_cam_container_name = f"{container_name}_{name}" + per_cam_container = ComposableNodeContainer( + name=per_cam_container_name, + namespace=namespace_val, + package='rclcpp_components', + executable=container_exec, + arguments=['--ros-args', '--log-level', 'info'], # keep single-threaded to reduce races + output='screen', + ) + actions.append(per_cam_container) + + # ZED Wrapper launch file + zed_wrapper_launch = IncludeLaunchDescription( + launch_description_source=PythonLaunchDescriptionSource([ + get_package_share_directory('zed_wrapper'), + '/launch/zed_camera.launch.py' + ]), + launch_arguments={ + 'container_name': per_cam_container_name, + 'camera_name': name, + 'camera_model': model, + 'serial_number': serial, + 'camera_id': id, + 'publish_tf': publish_tf, + 'publish_map_tf': publish_tf, + 'namespace': namespace_val + }.items() + ) + + # Load the component only after the per-camera container process is up + actions.append( + RegisterEventHandler( + OnProcessStart( + target_action=per_cam_container, + on_start=[ + # Slight stagger between cameras to be extra safe + TimerAction(period=0.3 * (cam_idx + 1), actions=[zed_wrapper_launch]) + ], + ) + ) + ) + + cam_idx += 1 + + return actions + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + 'cam_names', + description='An array containing the name of the cameras, e.g. [zed_front,zed_back]'), + DeclareLaunchArgument( + 'cam_models', + description='An array containing the model of the cameras, e.g. [zed2i,zed2]'), + DeclareLaunchArgument( + 'cam_serials', + default_value=[], + description='An array containing the serial number of the cameras, e.g. [35199186,23154724]'), + DeclareLaunchArgument( + 'cam_ids', + default_value=[], + description='An array containing the ID number of the cameras, e.g. [0,1]'), + + OpaqueFunction(function=launch_setup) + ] + ) \ No newline at end of file