From 90294331110c066f5965d3239e9367aaf3539905 Mon Sep 17 00:00:00 2001 From: "Ortiz Cuesta, Fernando" Date: Tue, 14 May 2024 10:14:32 +0200 Subject: [PATCH 1/3] Allow hw synchronization of several realsense devices --- .../include/base_realsense_node.h | 1 + realsense2_camera/include/constants.h | 1 + realsense2_camera/include/ros_sensor.h | 4 +- realsense2_camera/launch/rs_launch.py | 1 + .../launch/rs_multi_camera_launch_sync.py | 82 +++++++++++++++++++ realsense2_camera/src/base_realsense_node.cpp | 1 + realsense2_camera/src/parameters.cpp | 5 ++ realsense2_camera/src/ros_sensor.cpp | 21 ++++- realsense2_camera/src/rs_node_setup.cpp | 2 +- 9 files changed, 115 insertions(+), 3 deletions(-) create mode 100644 realsense2_camera/launch/rs_multi_camera_launch_sync.py diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4792f5456c..4e3c76ada3 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -318,6 +318,7 @@ namespace realsense2_camera rclcpp::Time _ros_time_base; bool _sync_frames; + int _inter_cam_sync_mode; bool _enable_rgbd; bool _is_color_enabled; bool _is_depth_enabled; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 8304dbea4a..d3688464d4 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -69,6 +69,7 @@ namespace realsense2_camera const bool ALLOW_NO_TEXTURE_POINTS = false; const bool ORDERED_PC = false; const bool SYNC_FRAMES = false; + const int _INTER_CAM_SYNC_MODE = 0; const bool ENABLE_RGBD = false; const bool PUBLISH_TF = true; diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h index 57d224300e..b07f39941f 100644 --- a/realsense2_camera/include/ros_sensor.h +++ b/realsense2_camera/include/ros_sensor.h @@ -82,7 +82,8 @@ namespace realsense2_camera std::shared_ptr diagnostics_updater, rclcpp::Logger logger, bool force_image_default_qos = false, - bool is_rosbag_file = false); + bool is_rosbag_file = false, + int inter_cam_sync_mode = 0); ~RosSensor(); void registerSensorParameters(); bool getUpdatedProfiles(std::vector& wanted_profiles); @@ -105,6 +106,7 @@ namespace realsense2_camera void set_sensor_auto_exposure_roi(); void registerAutoExposureROIOptions(); void UpdateSequenceIdCallback(); + void set_inter_cam_sync_mode(int inter_cam_sync_mode = 0); template void set_sensor_parameter_to_ros(rs2_option option); diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index bdbdfd0ac8..d0fcd48dae 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -56,6 +56,7 @@ {'name': 'depth_module.exposure.2', 'default': '1', 'description': 'Depth module second exposure value. Used for hdr_merge filter'}, {'name': 'depth_module.gain.2', 'default': '16', 'description': 'Depth module second gain value. Used for hdr_merge filter'}, {'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"}, + {'name': 'inter_cam_sync_mode', 'default': "0", 'description': '[0-Default, 1-Master, 2-Slave]'}, {'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"}, {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, diff --git a/realsense2_camera/launch/rs_multi_camera_launch_sync.py b/realsense2_camera/launch/rs_multi_camera_launch_sync.py new file mode 100644 index 0000000000..bb6a22ffe0 --- /dev/null +++ b/realsense2_camera/launch/rs_multi_camera_launch_sync.py @@ -0,0 +1,82 @@ +# Copyright 2023 Intel Corporation. All Rights Reserved. +# +# 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. + +# DESCRIPTION # +# ----------- # +# Use this launch file to launch 2 devices and enable the hardware synchronization. +# As describe in https://dev.intelrealsense.com/docs/multiple-depth-cameras-configuration both devices +# have to be connected using a sync cable. The devices will by default stream asynchronously. +# Using this launch file one device will operate as master and the other as slave. As a result they will +# capture at exactly the same time and rate. +# command line example: (to be adapted with the serial numbers or the used devices) +# ros2 launch realsense2_camera rs_multi_camera_launch_sync.py camera_name1:=cam_1 camera_name2:=cam_2 camera_namespace1:=camera camera_namespace2:=camera serial_no1:="'_031422250097'" serial_no2:="'_336222301921'" + +"""Launch realsense2_camera node.""" +import copy +from launch import LaunchDescription, LaunchContext +import launch_ros.actions +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +from launch.launch_description_sources import PythonLaunchDescriptionSource +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.absolute())) +import rs_launch + +local_parameters = [{'name': 'camera_name1', 'default': 'camera1', 'description': 'camera1 unique name'}, + {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera2 unique name'}, + {'name': 'camera_namespace1', 'default': 'camera1', 'description': 'camera1 namespace'}, + {'name': 'camera_namespace2', 'default': 'camera2', 'description': 'camera2 namespace'}, + {'name': 'inter_cam_sync_mode1', 'default': '1', 'description': 'master'}, + {'name': 'inter_cam_sync_mode2', 'default': '2', 'description': 'slave'}, + ] + + +def set_configurable_parameters(local_params): + return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params]) + +def duplicate_params(general_params, posix): + local_params = copy.deepcopy(general_params) + for param in local_params: + param['original_name'] = param['name'] + param['name'] += posix + return local_params + +def launch_static_transform_publisher_node(context : LaunchContext): + # dummy static transformation from camera1 to camera2 + node = launch_ros.actions.Node( + package = "tf2_ros", + executable = "static_transform_publisher", + arguments = ["0", "0", "0", "0", "0", "0", + context.launch_configurations['camera_name1'] + "_link", + context.launch_configurations['camera_name2'] + "_link"] + ) + return [node] + +def generate_launch_description(): + params1 = duplicate_params(rs_launch.configurable_parameters, '1') + params2 = duplicate_params(rs_launch.configurable_parameters, '2') + return LaunchDescription( + rs_launch.declare_configurable_parameters(local_parameters) + + rs_launch.declare_configurable_parameters(params1) + + rs_launch.declare_configurable_parameters(params2) + + [ + OpaqueFunction(function=rs_launch.launch_setup, + kwargs = {'params' : set_configurable_parameters(params1), + 'param_name_suffix': '1'}), + OpaqueFunction(function=rs_launch.launch_setup, + kwargs = {'params' : set_configurable_parameters(params2), + 'param_name_suffix': '2'}), + OpaqueFunction(function=launch_static_transform_publisher_node) + ]) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 4b88ec7df1..c5a7e7e24f 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -110,6 +110,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_initialized_time_base(false), _camera_time_base(0), _sync_frames(SYNC_FRAMES), + _inter_cam_sync_mode(_INTER_CAM_SYNC_MODE), _enable_rgbd(ENABLE_RGBD), _is_color_enabled(false), _is_depth_enabled(false), diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index dda0b6133e..b91d3531e2 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -47,6 +47,11 @@ void BaseRealSenseNode::getParameters() _parameters->setParamT(param_name, _sync_frames); _parameters_names.push_back(param_name); + param_name = std::string("inter_cam_sync_mode"); + _parameters->setParamT(param_name, _inter_cam_sync_mode); + _parameters_names.push_back(param_name); + + param_name = std::string("enable_rgbd"); _parameters->setParamT(param_name, _enable_rgbd, [this](const rclcpp::Parameter& ) { diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index f0a5aa5331..21e5f174e3 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -46,7 +46,8 @@ RosSensor::RosSensor(rs2::sensor sensor, std::shared_ptr diagnostics_updater, rclcpp::Logger logger, bool force_image_default_qos, - bool is_rosbag_file): + bool is_rosbag_file, + int inter_cam_sync_mode): rs2::sensor(sensor), _logger(logger), _origin_frame_callback(frame_callback), @@ -75,6 +76,7 @@ RosSensor::RosSensor(rs2::sensor sensor, } }; setParameters(is_rosbag_file); + set_inter_cam_sync_mode(inter_cam_sync_mode); } RosSensor::~RosSensor() @@ -108,6 +110,23 @@ void RosSensor::setParameters(bool is_rosbag_file) registerSensorParameters(); } +void RosSensor::set_inter_cam_sync_mode(int inter_cam_sync_mode) +{ + std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); + + // Only depth_module supports the option RS2_OPTION_INTER_CAM_SYNC_MODE, modules rgb_camera and motion_module + // throw an error when trying to set the option RS2_OPTION_INTER_CAM_SYNC_MODE + if (module_name == std::string("depth_module")){ + if ((0 <= inter_cam_sync_mode) && (2 >= inter_cam_sync_mode)) + { + ROS_INFO_STREAM("Set Ros param depth_module.inter_cam_sync_mode to " << inter_cam_sync_mode << " (0=Default, 1=Master and 2=slave)"); + set_option(RS2_OPTION_INTER_CAM_SYNC_MODE, inter_cam_sync_mode); + } else { + ROS_ERROR_STREAM("Ros param depth_module.inter_cam_sync_mode was not set. Given value "<< inter_cam_sync_mode << " is invalid"); + } + } +} + void RosSensor::UpdateSequenceIdCallback() { // Function replaces the trivial parameter callback with one that diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 191644b83e..0f5712ec82 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -159,7 +159,7 @@ void BaseRealSenseNode::setAvailableSensors() sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor."); - rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is()); + rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is(), _inter_cam_sync_mode); } else if (sensor.is()) { From 41d9bab5ede40903fc49d5a0e52fa5420cd9fc00 Mon Sep 17 00:00:00 2001 From: "Ortiz Cuesta, Fernando" Date: Tue, 14 May 2024 16:31:10 +0200 Subject: [PATCH 2/3] Revert changes --- .../include/base_realsense_node.h | 1 - realsense2_camera/include/constants.h | 1 - realsense2_camera/include/ros_sensor.h | 4 +--- realsense2_camera/src/base_realsense_node.cpp | 1 - realsense2_camera/src/parameters.cpp | 5 ----- realsense2_camera/src/ros_sensor.cpp | 21 +------------------ realsense2_camera/src/rs_node_setup.cpp | 2 +- 7 files changed, 3 insertions(+), 32 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4e3c76ada3..4792f5456c 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -318,7 +318,6 @@ namespace realsense2_camera rclcpp::Time _ros_time_base; bool _sync_frames; - int _inter_cam_sync_mode; bool _enable_rgbd; bool _is_color_enabled; bool _is_depth_enabled; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index d3688464d4..8304dbea4a 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -69,7 +69,6 @@ namespace realsense2_camera const bool ALLOW_NO_TEXTURE_POINTS = false; const bool ORDERED_PC = false; const bool SYNC_FRAMES = false; - const int _INTER_CAM_SYNC_MODE = 0; const bool ENABLE_RGBD = false; const bool PUBLISH_TF = true; diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h index b07f39941f..57d224300e 100644 --- a/realsense2_camera/include/ros_sensor.h +++ b/realsense2_camera/include/ros_sensor.h @@ -82,8 +82,7 @@ namespace realsense2_camera std::shared_ptr diagnostics_updater, rclcpp::Logger logger, bool force_image_default_qos = false, - bool is_rosbag_file = false, - int inter_cam_sync_mode = 0); + bool is_rosbag_file = false); ~RosSensor(); void registerSensorParameters(); bool getUpdatedProfiles(std::vector& wanted_profiles); @@ -106,7 +105,6 @@ namespace realsense2_camera void set_sensor_auto_exposure_roi(); void registerAutoExposureROIOptions(); void UpdateSequenceIdCallback(); - void set_inter_cam_sync_mode(int inter_cam_sync_mode = 0); template void set_sensor_parameter_to_ros(rs2_option option); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index c5a7e7e24f..4b88ec7df1 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -110,7 +110,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_initialized_time_base(false), _camera_time_base(0), _sync_frames(SYNC_FRAMES), - _inter_cam_sync_mode(_INTER_CAM_SYNC_MODE), _enable_rgbd(ENABLE_RGBD), _is_color_enabled(false), _is_depth_enabled(false), diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index b91d3531e2..dda0b6133e 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -47,11 +47,6 @@ void BaseRealSenseNode::getParameters() _parameters->setParamT(param_name, _sync_frames); _parameters_names.push_back(param_name); - param_name = std::string("inter_cam_sync_mode"); - _parameters->setParamT(param_name, _inter_cam_sync_mode); - _parameters_names.push_back(param_name); - - param_name = std::string("enable_rgbd"); _parameters->setParamT(param_name, _enable_rgbd, [this](const rclcpp::Parameter& ) { diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 21e5f174e3..f0a5aa5331 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -46,8 +46,7 @@ RosSensor::RosSensor(rs2::sensor sensor, std::shared_ptr diagnostics_updater, rclcpp::Logger logger, bool force_image_default_qos, - bool is_rosbag_file, - int inter_cam_sync_mode): + bool is_rosbag_file): rs2::sensor(sensor), _logger(logger), _origin_frame_callback(frame_callback), @@ -76,7 +75,6 @@ RosSensor::RosSensor(rs2::sensor sensor, } }; setParameters(is_rosbag_file); - set_inter_cam_sync_mode(inter_cam_sync_mode); } RosSensor::~RosSensor() @@ -110,23 +108,6 @@ void RosSensor::setParameters(bool is_rosbag_file) registerSensorParameters(); } -void RosSensor::set_inter_cam_sync_mode(int inter_cam_sync_mode) -{ - std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); - - // Only depth_module supports the option RS2_OPTION_INTER_CAM_SYNC_MODE, modules rgb_camera and motion_module - // throw an error when trying to set the option RS2_OPTION_INTER_CAM_SYNC_MODE - if (module_name == std::string("depth_module")){ - if ((0 <= inter_cam_sync_mode) && (2 >= inter_cam_sync_mode)) - { - ROS_INFO_STREAM("Set Ros param depth_module.inter_cam_sync_mode to " << inter_cam_sync_mode << " (0=Default, 1=Master and 2=slave)"); - set_option(RS2_OPTION_INTER_CAM_SYNC_MODE, inter_cam_sync_mode); - } else { - ROS_ERROR_STREAM("Ros param depth_module.inter_cam_sync_mode was not set. Given value "<< inter_cam_sync_mode << " is invalid"); - } - } -} - void RosSensor::UpdateSequenceIdCallback() { // Function replaces the trivial parameter callback with one that diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 0f5712ec82..191644b83e 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -159,7 +159,7 @@ void BaseRealSenseNode::setAvailableSensors() sensor.is()) { ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor."); - rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is(), _inter_cam_sync_mode); + rosSensor = std::make_unique(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is()); } else if (sensor.is()) { From a6c90ff36e0eed86ba600c1d69030a993df3da09 Mon Sep 17 00:00:00 2001 From: "Ortiz Cuesta, Fernando" Date: Tue, 14 May 2024 16:32:32 +0200 Subject: [PATCH 3/3] Expose depth_module.inter_cam_sync_mode in launch files --- realsense2_camera/launch/rs_launch.py | 2 +- realsense2_camera/launch/rs_multi_camera_launch_sync.py | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index d0fcd48dae..7f5202c811 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -56,7 +56,7 @@ {'name': 'depth_module.exposure.2', 'default': '1', 'description': 'Depth module second exposure value. Used for hdr_merge filter'}, {'name': 'depth_module.gain.2', 'default': '16', 'description': 'Depth module second gain value. Used for hdr_merge filter'}, {'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"}, - {'name': 'inter_cam_sync_mode', 'default': "0", 'description': '[0-Default, 1-Master, 2-Slave]'}, + {'name': 'depth_module.inter_cam_sync_mode', 'default': "0", 'description': '[0-Default, 1-Master, 2-Slave]'}, {'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"}, {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, diff --git a/realsense2_camera/launch/rs_multi_camera_launch_sync.py b/realsense2_camera/launch/rs_multi_camera_launch_sync.py index bb6a22ffe0..de127d35fb 100644 --- a/realsense2_camera/launch/rs_multi_camera_launch_sync.py +++ b/realsense2_camera/launch/rs_multi_camera_launch_sync.py @@ -18,9 +18,10 @@ # As describe in https://dev.intelrealsense.com/docs/multiple-depth-cameras-configuration both devices # have to be connected using a sync cable. The devices will by default stream asynchronously. # Using this launch file one device will operate as master and the other as slave. As a result they will -# capture at exactly the same time and rate. +# capture at exactly the same time and rate. # command line example: (to be adapted with the serial numbers or the used devices) # ros2 launch realsense2_camera rs_multi_camera_launch_sync.py camera_name1:=cam_1 camera_name2:=cam_2 camera_namespace1:=camera camera_namespace2:=camera serial_no1:="'_031422250097'" serial_no2:="'_336222301921'" +# The value of the param can be checked using ros2 param get /camera/cam_1 depth_module.inter_cam_sync_mode and ros2 param get /camera/cam_2 depth_module.inter_cam_sync_mode """Launch realsense2_camera node.""" import copy @@ -38,8 +39,8 @@ {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera2 unique name'}, {'name': 'camera_namespace1', 'default': 'camera1', 'description': 'camera1 namespace'}, {'name': 'camera_namespace2', 'default': 'camera2', 'description': 'camera2 namespace'}, - {'name': 'inter_cam_sync_mode1', 'default': '1', 'description': 'master'}, - {'name': 'inter_cam_sync_mode2', 'default': '2', 'description': 'slave'}, + {'name': 'depth_module.inter_cam_sync_mode1', 'default': '1', 'description': 'master'}, + {'name': 'depth_module.inter_cam_sync_mode2', 'default': '2', 'description': 'slave'}, ]