From ac85afd14ae4b18ac4133d1945ca24135cd84cde Mon Sep 17 00:00:00 2001 From: Xiangyu Date: Fri, 15 Apr 2022 10:53:42 +0800 Subject: [PATCH 01/85] add D430i support --- realsense2_camera/include/constants.h | 1 + realsense2_camera/src/realsense_node_factory.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index de5ecb8480..895e167fb7 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -66,6 +66,7 @@ namespace realsense2_camera const uint16_t RS435i_RGB_PID = 0x0B3A; // AWGC_MM const uint16_t RS465_PID = 0x0b4d; // D465 const uint16_t RS416_RGB_PID = 0x0B52; // F416 RGB + const uint16_t RS430i_PID = 0x0b4b; // D430i const uint16_t RS405_PID = 0x0B5B; // DS5U const uint16_t RS455_PID = 0x0B5C; // D455 const uint16_t RS_T265_PID = 0x0b37; // diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 2a8fc31aa1..375b0819ee 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -366,6 +366,7 @@ void RealSenseNodeFactory::startDevice() case RS420_PID: case RS420_MM_PID: case RS430_PID: + case RS430i_PID: case RS430_MM_PID: case RS430_MM_RGB_PID: case RS435_RGB_PID: From a554c7ca937f3bf623186e7585f67a170b29e92c Mon Sep 17 00:00:00 2001 From: Jeremy Shannon Date: Thu, 7 Jul 2022 12:14:47 -0400 Subject: [PATCH 02/85] Update README.md update rosdep due to Eloquent going EOL --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index aec9f15a85..e023990f8b 100644 --- a/README.md +++ b/README.md @@ -63,8 +63,8 @@ LibRealSense supported version: v2.50.0 (see [realsense2_camera release notes](h ### Step 4: Install dependencies ```bash sudo apt-get install python3-rosdep -y - sudo rosdep init # "sudo rosdep init --include-eol-distros" for Dashing - rosdep update + sudo rosdep init # "sudo rosdep init --include-eol-distros" for Eloquent and earlier + rosdep update # "sudo rosdep update --include-eol-distros" for Eloquent and earlier rosdep install -i --from-path src --rosdistro $ROS_DISTRO --skip-keys=librealsense2 -y ``` From 223ed72bb6ae73cb0db8ddfc7c6216bfd5245f28 Mon Sep 17 00:00:00 2001 From: Gilad Bretter <7240093+Gilaadb@users.noreply.github.com> Date: Tue, 2 Aug 2022 11:49:10 +0300 Subject: [PATCH 03/85] Publish static tf for IMU frames {camera}_imu_frame and {camera}_imu_optical_frame were not published. The tf is the same as the Gyro's and Accelerometer's. Now publishing IMU frames tf when ever publishing gyro's tf. --- realsense2_camera/include/base_realsense_node.h | 2 ++ realsense2_camera/src/base_realsense_node.cpp | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 27024b14f8..934f81ef2f 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -42,6 +42,8 @@ using realsense2_camera_msgs::msg::Extrinsics; using realsense2_camera_msgs::msg::IMUInfo; #define FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_frame")).str() +#define IMU_FRAME_ID (static_cast(std::ostringstream() << _camera_name << "_imu_frame")).str() +#define IMU_OPTICAL_FRAME_ID (static_cast(std::ostringstream() << _camera_name << "_imu_optical_frame")).str() #define OPTICAL_FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_optical_frame")).str() #define ALIGNED_DEPTH_TO_FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << "aligned_depth_to_" << STREAM_NAME(sip) << "_frame")).str() diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 4cf8cd6941..9efc92b7fa 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -837,6 +837,8 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& tf2::Quaternion quaternion_optical; quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2); float3 zero_trans{0, 0, 0}; + tf2::Quaternion zero_rot_quaternions; + zero_rot_quaternions.setRPY(0, 0, 0); rclcpp::Time transform_ts_ = _node.now(); @@ -872,6 +874,13 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& publish_static_tf(transform_ts_, trans, Q, _base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); publish_static_tf(transform_ts_, zero_trans, quaternion_optical, ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); } + + if (profile.stream_type() == RS2_STREAM_GYRO) + { + publish_static_tf(transform_ts_, zero_trans, zero_rot_quaternions, FRAME_ID(sip), IMU_FRAME_ID); + publish_static_tf(transform_ts_, zero_trans, quaternion_optical, IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); + } + publishExtrinsicsTopic(sip, ex); } From 7d376fa3eff1f7c41d5cd06ed9371da125aa782c Mon Sep 17 00:00:00 2001 From: Gilad Bretter <7240093+Gilaadb@users.noreply.github.com> Date: Tue, 13 Sep 2022 15:27:56 +0300 Subject: [PATCH 04/85] publish IMU frames only if unite/sync imu method is not none --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9efc92b7fa..17123e58ae 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -875,7 +875,7 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& publish_static_tf(transform_ts_, zero_trans, quaternion_optical, ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); } - if (profile.stream_type() == RS2_STREAM_GYRO) + if ((_imu_sync_method > imu_sync_method::NONE) && (profile.stream_type() == RS2_STREAM_GYRO)) { publish_static_tf(transform_ts_, zero_trans, zero_rot_quaternions, FRAME_ID(sip), IMU_FRAME_ID); publish_static_tf(transform_ts_, zero_trans, quaternion_optical, IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); From 61406afa1e32b024992606626d29e57258590640 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 13 Sep 2022 17:10:57 +0300 Subject: [PATCH 05/85] fix extrinsics calculation --- realsense2_camera/src/base_realsense_node.cpp | 2 +- realsense2_camera/src/t265_realsense_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 81c318099d..a37c92629f 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -850,7 +850,7 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& rs2_extrinsics ex; try { - ex = profile.get_extrinsics_to(base_profile); + ex = base_profile.get_extrinsics_to(profile); } catch (std::exception& e) { diff --git a/realsense2_camera/src/t265_realsense_node.cpp b/realsense2_camera/src/t265_realsense_node.cpp index 20b198e1e0..523363282d 100644 --- a/realsense2_camera/src/t265_realsense_node.cpp +++ b/realsense2_camera/src/t265_realsense_node.cpp @@ -89,7 +89,7 @@ void T265RealsenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& rs2_extrinsics ex; try { - ex = profile.get_extrinsics_to(base_profile); + ex = base_profile.get_extrinsics_to(profile); } catch (std::exception& e) { From ef25e043b3221317353a26322ae4f6bf17a4137b Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 21 Sep 2022 14:47:07 +0300 Subject: [PATCH 06/85] fix ordered_pc arg prefix --- realsense2_camera/launch/rs_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 0bc2ec3e6d..9ec7c52403 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -47,7 +47,7 @@ {'name': 'linear_accel_cov', 'default': '0.01', 'description': "''"}, {'name': 'initial_reset', 'default': 'false', 'description': "''"}, {'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"}, - {'name': 'ordered_pc', 'default': 'false', 'description': ''}, + {'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''}, {'name': 'calib_odom_file', 'default': "''", 'description': "''"}, {'name': 'topic_odom_in', 'default': "''", 'description': 'topic for T265 wheel odometry'}, {'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'}, From b7560d7677980fb023f3fcf97178c7b0dfdbae9b Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 29 Sep 2022 12:04:48 +0300 Subject: [PATCH 07/85] add humble distro to pre-release.yml --- .github/workflows/pre-release.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index adf299bb1b..5a99ffa429 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -31,8 +31,10 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [galactic, foxy] + ros_distro: [humble,galactic, foxy] include: + - ros_distro: 'humble' + os: ubuntu-22.04 - ros_distro: 'galactic' os: ubuntu-20.04 - ros_distro: 'foxy' From 558f7e52db1536be269820422427e067e960c498 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 29 Sep 2022 12:29:23 +0300 Subject: [PATCH 08/85] add space --- .github/workflows/pre-release.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 5a99ffa429..1306fa61eb 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -31,7 +31,7 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [humble,galactic, foxy] + ros_distro: [humble, galactic, foxy] include: - ros_distro: 'humble' os: ubuntu-22.04 From 882a2fa3810a709a8c778a0245f0ea3046854101 Mon Sep 17 00:00:00 2001 From: Gilad Bretter <7240093+Gilaadb@users.noreply.github.com> Date: Wed, 28 Sep 2022 17:12:24 +0300 Subject: [PATCH 09/85] publish actual IMU optical frame ID --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index efd8014ba4..328eb534f4 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -330,7 +330,7 @@ void BaseRealSenseNode::FillImuData_Copy(const CimuData imu_data, std::deque Date: Mon, 31 Oct 2022 13:28:35 +0200 Subject: [PATCH 10/85] rename ros2-beta to ros2-development --- .github/workflows/main.yml | 6 +++--- .github/workflows/pre-release.yml | 4 ++-- README.md | 11 +++++++---- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 308246a4fa..fbceca5ee5 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -2,13 +2,13 @@ name: CI # Controls when the workflow will run on: - # Triggers the workflow on push or pull request events but only for the ros2-beta branch + # Triggers the workflow on push or pull request events but only for the ros2-development branch push: branches: - - ros2-beta + - ros2-development pull_request: branches: - - ros2-beta + - ros2-development # Allows you to run this workflow manually from the Actions tab workflow_dispatch: diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index adf299bb1b..bccbb0c95e 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -16,10 +16,10 @@ on: # Triggers the workflow on push or pull request events but only for the ros2-beta branch push: branches: - - ros2-beta + - ros2-development pull_request: branches: - - ros2-beta + - ros2-development # Allows you to run this workflow manually from the Actions tab workflow_dispatch: diff --git a/README.md b/README.md index e121cb2e42..d6aacf4e78 100644 --- a/README.md +++ b/README.md @@ -5,13 +5,16 @@ This version supports ROS2 Dashing, Eloquent, Foxy, Galactic and Rolling. LibRealSense supported version: v2.51.1 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) -## Please notice: if you are moving from RealSense [ROS2 branch](https://github.com/IntelRealSense/realsense-ros/tree/ros2) to ROS2-beta: + +## For LibRS ROS1 Wrapper please refer to [ROS1-legacy branch](https://github.com/IntelRealSense/realsense-ros/tree/ros1-legacy) + +## Please notice: if you are moving from RealSense [ROS2-legacy branch](https://github.com/IntelRealSense/realsense-ros/tree/ros2-legacy) to ROS2-development: - **Changed Parameters**: - **"stereo_module"**, **"l500_depth_sensor"** are replaced by **"depth_module"** - For video streams: **\.profile** replaces **\_width**, **\_height**, **\_fps** - - **ROS2 (Old)**: + - **ROS2-legacy (Old)**: - ros2 launch realsense2_camera rs_launch.py depth_width:=640 depth_height:=480 depth_fps:=30.0 infra1_width:=640 infra1_height:=480 infra1_fps:=30.0 - - **ROS2-beta (New)**: + - **ROS2-development (New)**: - ros2 launch realsense2_camera rs_launch.py depth_module.profile:=640x480x30 - Removed paramets **\_frame_id**, **\_optical_frame_id**. frame_ids are now defined by camera_name - **"filters"** is removed. All filters (or post-processing blocks) are enabled/disabled using **"\.enable"** @@ -58,7 +61,7 @@ LibRealSense supported version: v2.51.1 (see [realsense2_camera release notes](h ``` - Clone the latest ROS2 Intel® RealSense™ wrapper from [here](https://github.com/IntelRealSense/realsense-ros.git) into '~/ros2_ws/src/' ```bashrc - git clone https://github.com/IntelRealSense/realsense-ros.git -b ros2-beta + git clone https://github.com/IntelRealSense/realsense-ros.git -b ros2-development cd ~/ros2_ws ``` ### Step 4: Install dependencies From b9772241d833ec86b3867cc5403c3564086ca66b Mon Sep 17 00:00:00 2001 From: Jaiveer Singh <84546269+jaiveersinghNV@users.noreply.github.com> Date: Tue, 8 Nov 2022 14:29:08 -0800 Subject: [PATCH 11/85] Update stale comment to reflect new branch name Seems like there's a stale comment referring to the old, `ros2-beta` branch name, when it should instead reference the new, `ros2-development` branch. --- .github/workflows/pre-release.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 43e2913e77..f27237ea26 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -13,7 +13,7 @@ name: pre-release # Controls when the workflow will run on: - # Triggers the workflow on push or pull request events but only for the ros2-beta branch + # Triggers the workflow on push or pull request events but only for the ros2-development branch push: branches: - ros2-development From c80c924eac34f55df60ff9abef2a67c4b9eaa5c4 Mon Sep 17 00:00:00 2001 From: Pranav Dhulipala Date: Tue, 8 Nov 2022 16:28:42 -0800 Subject: [PATCH 12/85] Windows bringup --- realsense2_camera/CMakeLists.txt | 4 ++++ realsense2_camera/src/realsense_node_factory.cpp | 2 ++ 2 files changed, 6 insertions(+) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 84c9e5bd65..89c11ada74 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -59,6 +59,10 @@ if(UNIX OR APPLE) set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie") endif() +if(WIN32) + add_definitions(-D_USE_MATH_DEFINES) +endif() + if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 2a8fc31aa1..9c071b366c 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -10,7 +10,9 @@ #include #include #include +#ifndef _WIN32 #include +#endif #include using namespace realsense2_camera; From 24e01f1d79ae7cc16d69b76c7aec12ede7efb1e2 Mon Sep 17 00:00:00 2001 From: augustelalande Date: Fri, 11 Nov 2022 16:03:59 -0500 Subject: [PATCH 13/85] replace deprecated parameter node_name with name --- .../launch/view_model.launch.py | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/realsense2_description/launch/view_model.launch.py b/realsense2_description/launch/view_model.launch.py index 25f5736995..0b0310023e 100644 --- a/realsense2_description/launch/view_model.launch.py +++ b/realsense2_description/launch/view_model.launch.py @@ -11,33 +11,34 @@ sys.path.append(os.path.dirname(os.path.realpath(__file__))) from launch_utils import to_urdf + def generate_launch_description(): available_urdf_files = [f for f in os.listdir(os.path.join(get_package_share_directory('realsense2_description'), 'urdf')) if f.startswith('test_')] - params = dict([aa for aa in [aa.split(':=') for aa in sys.argv] if len(aa)==2]) + params = dict([aa for aa in [aa.split(':=') for aa in sys.argv] if len(aa) == 2]) if ('model' not in params or params['model'] not in available_urdf_files): - print ('USAGE:') - print ('ros2 launch realsense2_description view_model.launch.py model:=') - print ('Available argumants for are as follows:') - print ('\n'.join(available_urdf_files)) + print('USAGE:') + print('ros2 launch realsense2_description view_model.launch.py model:=') + print('Available argumants for are as follows:') + print('\n'.join(available_urdf_files)) return launch.LaunchDescription() rviz_config_dir = os.path.join(get_package_share_directory('realsense2_description'), 'rviz', 'urdf.rviz') xacro_path = os.path.join(get_package_share_directory('realsense2_description'), 'urdf', params['model']) - urdf = to_urdf(xacro_path, {'use_nominal_extrinsics' : 'true', 'add_plug' : 'true'}) + urdf = to_urdf(xacro_path, {'use_nominal_extrinsics': 'true', 'add_plug': 'true'}) rviz_node = Node( package='rviz2', executable='rviz2', - node_name='rviz2', - output = 'screen', + name='rviz2', + output='screen', arguments=['-d', rviz_config_dir], parameters=[{'use_sim_time': False}] ) model_node = Node( - node_name='model_node', + name='model_node', package='robot_state_publisher', executable='robot_state_publisher', namespace='', output='screen', - arguments = [urdf] + arguments=[urdf] ) return launch.LaunchDescription([rviz_node, model_node]) From 3c6bdf65777c083cbb7994a24896485387b3e783 Mon Sep 17 00:00:00 2001 From: Adarsh TS Date: Fri, 18 Nov 2022 06:20:30 +0530 Subject: [PATCH 14/85] Explicit mention of parameter value for `unite_imu_method` There is confusion regarding what value needs to be passed to `unite_imu_method`. The `ros2-development` branch expects a different value for the `unite_imu_method` parameter as compared to the `ros2-legacy` and `ros1-legacy` branches. I have added this detail in the README. Thanks to @Nir-Az for pointing this out in [this](https://github.com/IntelRealSense/realsense-ros/issues/2542#issuecomment-1318831114) issue. --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index d6aacf4e78..91c026d345 100644 --- a/README.md +++ b/README.md @@ -232,8 +232,8 @@ For setting a new value for a parameter use `ros2 param set Date: Wed, 16 Nov 2022 17:45:12 +0200 Subject: [PATCH 15/85] add rolling to gha --- .github/workflows/main.yml | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index fbceca5ee5..a0777f17fb 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -23,8 +23,10 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [humble, galactic, foxy, eloquent, dashing] + ros_distro: [rolling, humble, galactic, foxy, eloquent, dashing] include: + - ros_distro: 'rolling' + os: ubuntu-22.04 - ros_distro: 'humble' os: ubuntu-22.04 - ros_distro: 'galactic' @@ -67,20 +69,21 @@ jobs: ## See: 1) https://github.com/ros-tooling/setup-ros#Supported-platforms ## 2) https://github.com/ros-tooling/setup-ros/tree/v0.2#Supported-platforms - name: build ROS2 Galactic/Foxy/Eloquent/Dashing - if: ${{ matrix.ros_distro != 'humble'}} + if: ${{ matrix.ros_distro != 'humble' && matrix.ros_distro != 'rolling'}} uses: ros-tooling/setup-ros@v0.2 with: required-ros-distributions: ${{ matrix.ros_distro }} + - name: build ROS2 Humble - if: ${{ matrix.ros_distro == 'humble' }} + if: ${{ matrix.ros_distro == 'humble' || matrix.ros_distro == 'rolling' }} uses: ros-tooling/setup-ros@v0.3 with: required-ros-distributions: ${{ matrix.ros_distro }} - ## For all distros except humble, install RealSense SDK From Debinas - ## For Humble distro, install from source (TODO: Change this when we got debians for Humble) + ## For all distros except humble and rolling, install RealSense SDK From Debians + ## For humble + rolling distros, install from source (TODO: Change this when we have public librealsense2 debians for Ubuntu22) - name: Install RealSense SDK 2.0 Dependencies - if: ${{ matrix.ros_distro != 'humble' }} + if: ${{ matrix.ros_distro != 'humble' && matrix.ros_distro != 'rolling' }} run: | sudo apt-get update sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com --recv-key C8B3A55A6F3EFCDE @@ -88,8 +91,9 @@ jobs: sudo apt-get update -qq sudo apt-get install librealsense2-dev --allow-unauthenticated -y sudo apt-get update + - name: Build RealSense SDK 2.0 from source - if: ${{ matrix.ros_distro == 'humble' }} + if: ${{ matrix.ros_distro == 'humble' || matrix.ros_distro == 'rolling' }} run: | cd ${{github.workspace}} git clone https://github.com/IntelRealSense/librealsense.git -b master From 55ae9b9cd0ae00b8fe55363e37a5ae6a4b2e41ae Mon Sep 17 00:00:00 2001 From: nvidia Date: Mon, 21 Nov 2022 16:15:04 -0800 Subject: [PATCH 16/85] Add D457 --- realsense2_camera/include/constants.h | 1 + realsense2_camera/src/realsense_node_factory.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index f640bab854..0bf10f393f 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -68,6 +68,7 @@ namespace realsense2_camera const uint16_t RS416_RGB_PID = 0x0B52; // F416 RGB const uint16_t RS405_PID = 0x0B5B; // DS5U const uint16_t RS455_PID = 0x0B5C; // D455 + const uint16_t RS457_PID = 0xABCD; // D457 const uint16_t RS_T265_PID = 0x0b37; // const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // const uint16_t RS_L515_PID = 0x0B64; // diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 9c071b366c..adf4b5bd0d 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -373,6 +373,7 @@ void RealSenseNodeFactory::startDevice() case RS435_RGB_PID: case RS435i_RGB_PID: case RS455_PID: + case RS457_PID: case RS465_PID: case RS_USB2_PID: case RS_L515_PID_PRE_PRQ: From 911b2206bb53b9fe01c4ffed382a52159832018d Mon Sep 17 00:00:00 2001 From: NirAz Date: Tue, 22 Nov 2022 17:26:43 +0200 Subject: [PATCH 17/85] fix deprecated API --- realsense2_camera/CMakeLists.txt | 2 +- .../include/base_realsense_node.h | 6 ++++++ realsense2_camera/include/ros_param_backend.h | 10 ++++++++- .../src/ros_param_backend_dashing.cpp | 2 +- .../src/ros_param_backend_foxy.cpp | 2 +- .../src/ros_param_backend_rolling.cpp | 21 +++++++++++++++++++ 6 files changed, 39 insertions(+), 4 deletions(-) create mode 100644 realsense2_camera/src/ros_param_backend_rolling.cpp diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 89c11ada74..84de9fd5dc 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -156,7 +156,7 @@ elseif("$ENV{ROS_DISTRO}" STREQUAL "humble") elseif("$ENV{ROS_DISTRO}" STREQUAL "rolling") message(STATUS "Build for ROS2 Rolling") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DROLLING") - set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) + set(SOURCES "${SOURCES}" src/ros_param_backend_rolling.cpp) else() message(FATAL_ERROR "Unsupported ROS Distribution: " "$ENV{ROS_DISTRO}") endif() diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 934f81ef2f..4a88f9496e 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -6,7 +6,13 @@ #include #include #include "constants.h" + +// cv_bridge.h last supported version is humble +#if defined(ROLLING) +#include +#else #include +#endif #include #include diff --git a/realsense2_camera/include/ros_param_backend.h b/realsense2_camera/include/ros_param_backend.h index 74b70b95e3..aba3423cb8 100644 --- a/realsense2_camera/include/ros_param_backend.h +++ b/realsense2_camera/include/ros_param_backend.h @@ -15,7 +15,15 @@ namespace realsense2_camera _logger(node.get_logger()) {}; ~ParametersBackend(); - void add_on_set_parameters_callback(rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType callback); + + + #if defined( ROLLING ) + using ros2_param_callback_type = rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; + #else + using ros2_param_callback_type = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + #endif + + void add_on_set_parameters_callback(ros2_param_callback_type callback); private: diff --git a/realsense2_camera/src/ros_param_backend_dashing.cpp b/realsense2_camera/src/ros_param_backend_dashing.cpp index b76ff63e77..4afdb53b23 100644 --- a/realsense2_camera/src/ros_param_backend_dashing.cpp +++ b/realsense2_camera/src/ros_param_backend_dashing.cpp @@ -5,7 +5,7 @@ namespace realsense2_camera { - void ParametersBackend::add_on_set_parameters_callback(rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType callback) + void ParametersBackend::add_on_set_parameters_callback(ros2_param_callback_type callback) { rclcpp::Node::OnParametersSetCallbackType prev_callback = _node.set_on_parameters_set_callback(callback); if (prev_callback) diff --git a/realsense2_camera/src/ros_param_backend_foxy.cpp b/realsense2_camera/src/ros_param_backend_foxy.cpp index a36766b5b8..daceb7e171 100644 --- a/realsense2_camera/src/ros_param_backend_foxy.cpp +++ b/realsense2_camera/src/ros_param_backend_foxy.cpp @@ -5,7 +5,7 @@ namespace realsense2_camera { - void ParametersBackend::add_on_set_parameters_callback(rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType callback) + void ParametersBackend::add_on_set_parameters_callback(ros2_param_callback_type callback) { _ros_callback = _node.add_on_set_parameters_callback(callback); } diff --git a/realsense2_camera/src/ros_param_backend_rolling.cpp b/realsense2_camera/src/ros_param_backend_rolling.cpp new file mode 100644 index 0000000000..daceb7e171 --- /dev/null +++ b/realsense2_camera/src/ros_param_backend_rolling.cpp @@ -0,0 +1,21 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2022 Intel Corporation. All Rights Reserved. + +#include "ros_param_backend.h" + +namespace realsense2_camera +{ + void ParametersBackend::add_on_set_parameters_callback(ros2_param_callback_type callback) + { + _ros_callback = _node.add_on_set_parameters_callback(callback); + } + + ParametersBackend::~ParametersBackend() + { + if (_ros_callback) + { + _node.remove_on_set_parameters_callback((rclcpp::node_interfaces::OnSetParametersCallbackHandle*)(_ros_callback.get())); + _ros_callback.reset(); + } + } +} From 033ebcfe9f08242fe6e2b3d18c47ea293684862f Mon Sep 17 00:00:00 2001 From: Adarsh TS Date: Wed, 23 Nov 2022 18:16:43 +0530 Subject: [PATCH 18/85] Update `unite_imu_method` parameter description. Changes made as suggested by @Nir-Az --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 91c026d345..66c6f2f5cf 100644 --- a/README.md +++ b/README.md @@ -231,9 +231,9 @@ For setting a new value for a parameter use `ros2 param set None, 1 -> Copy, 2 -> Linear_ interpolation] when: + - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. + - **copy**: Every gyro message is attached by the last accel message. - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. From 9ec8f4fdc0213f3032285d6217e254e3762a921f Mon Sep 17 00:00:00 2001 From: Gilad Bretter <7240093+Gilaadb@users.noreply.github.com> Date: Wed, 7 Dec 2022 12:56:23 +0200 Subject: [PATCH 19/85] Properly read camera config files in rs_launch.py Modified rs_launch.py to properly read parameters from yaml config file. It did not read them before. Now we can set camera paramters that are not listen in rs_launch.py "configurable_parameters" list by specifying them in a yaml config file. Example for such parameters could be (among many others): depth_module.enable_auto_exposure depth_module.inter_cam_sync_mode --- realsense2_camera/launch/rs_launch.py | 65 ++++++++++----------------- 1 file changed, 24 insertions(+), 41 deletions(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 9ec7c52403..90459e2751 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -3,12 +3,11 @@ """Launch realsense2_camera node.""" import os +import yaml from launch import LaunchDescription -from ament_index_python.packages import get_package_share_directory import launch_ros.actions -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration configurable_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'}, @@ -68,63 +67,47 @@ def declare_configurable_parameters(parameters): def set_configurable_parameters(parameters): return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters]) -def generate_launch_description(): +def yaml_to_dict(path_to_yaml): + with open(path_to_yaml, "r") as f: + return yaml.load(f, Loader=yaml.SafeLoader) + +def launch_setup(context, *args, **kwargs): + _config_file = LaunchConfiguration("config_file").perform(context) + params_from_file = {} if _config_file == "''" else yaml_to_dict(_config_file) log_level = 'info' + # Realsense if (os.getenv('ROS_DISTRO') == "dashing") or (os.getenv('ROS_DISTRO') == "eloquent"): - return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [ - # Realsense - launch_ros.actions.Node( - condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " == ''"])), - package='realsense2_camera', - node_namespace=LaunchConfiguration("camera_name"), - node_name=LaunchConfiguration("camera_name"), - node_executable='realsense2_camera_node', - prefix=['stdbuf -o L'], - parameters=[set_configurable_parameters(configurable_parameters) - ], - output='screen', - arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], - ), + return [ launch_ros.actions.Node( - condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " != ''"])), package='realsense2_camera', node_namespace=LaunchConfiguration("camera_name"), node_name=LaunchConfiguration("camera_name"), node_executable='realsense2_camera_node', prefix=['stdbuf -o L'], parameters=[set_configurable_parameters(configurable_parameters) - , PythonExpression([LaunchConfiguration("config_file")]) + , params_from_file ], output='screen', arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], - ), - ]) + ) + ] else: - return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [ - # Realsense + return [ launch_ros.actions.Node( - condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " == ''"])), package='realsense2_camera', namespace=LaunchConfiguration("camera_name"), name=LaunchConfiguration("camera_name"), executable='realsense2_camera_node', parameters=[set_configurable_parameters(configurable_parameters) + , params_from_file ], output='screen', arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], emulate_tty=True, - ), - launch_ros.actions.Node( - condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " != ''"])), - package='realsense2_camera', - namespace=LaunchConfiguration("camera_name"), - name=LaunchConfiguration("camera_name"), - executable='realsense2_camera_node', - parameters=[set_configurable_parameters(configurable_parameters) - , PythonExpression([LaunchConfiguration("config_file")]) - ], - output='screen', - arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')], - emulate_tty=True, - ), - ]) + ) + ] + +def generate_launch_description(): + return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [ + OpaqueFunction(function=launch_setup) + ]) From e1a65bbbf9aa833c0ae7926b3ac3708dd3c87b17 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 17 Jan 2023 16:44:56 +0800 Subject: [PATCH 20/85] Publish static transformations for intra-process communication Signed-off-by: Yadunund --- README.md | 8 -------- realsense2_camera/src/base_realsense_node.cpp | 20 ++++++++++++++----- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 3d89df1fed..6f78cf632f 100644 --- a/README.md +++ b/README.md @@ -288,14 +288,6 @@ Further details on efficient intra-process communication can be found [here](htt ### Limitations * Node components are currently not supported on RCLPY -* Transformations: `/static_tf` topic will be disabled - * To get the transformations published: - * Set `tf_publish_rate` to `1.0` in the launch file (or on the command-line, using `-p tf_publish_rate:=1.0`) - * Activate and read `/tf` and `/extrinsic/_to_` topics - * To echo the `/extrinsic/_to_` topic you will need to change the default CLI QoS to match the new QoS that the `intra-process` flow uses. E.g.: - ```bash - ros2 topic echo /extrinsics/depth_to_color --qos-durability=volatile --qos-reliability=reliable - ``` * Compressed images using `image_transport` will be disabled as this isn't supported with intra-process communication ### Latency test tool and launch file diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 328eb534f4..671e794dfe 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -10,6 +10,13 @@ #include #include +// Header files for disabling intra-process comms for static broadcaster. +#include +// This header file is not available in ROS 2 Dashing. +#ifndef DASHING +#include +#endif + using namespace realsense2_camera; SyncedImuPublisher::SyncedImuPublisher(rclcpp::Publisher::SharedPtr imu_publisher, @@ -91,11 +98,14 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, { ROS_INFO("Intra-Process communication enabled"); } - else - { - // intra-process requirment of QoS.durability=Volatile cannot be fulfilled with `StaticTransformBroadcaster` as it only support `TransientLocal` durability. - _static_tf_broadcaster = std::make_shared(node); - } + + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + #ifndef DASHING + _static_tf_broadcaster = std::make_shared(node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); + #else + _static_tf_broadcaster = std::make_shared(node, rclcpp::QoS(100), std::move(options)); + #endif _image_format[1] = CV_8UC1; // CVBridge type _image_format[2] = CV_16UC1; // CVBridge type From 60f758394cd5984dcb010fd20edc14e48d89b7df Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 18 Jan 2023 15:51:31 +0200 Subject: [PATCH 21/85] Fix ros2 sensor controls steps and add control default value to param description --- realsense2_camera/src/sensor_params.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index b6b26d2f0f..4a23aa51b0 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -102,12 +102,13 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; std::stringstream desc; desc << sensor.get_option_description(option) << std::endl << description_addition; - crnt_descriptor.description = desc.str(); if (std::is_same::value || std::is_same::value) { rcl_interfaces::msg::IntegerRange range; range.from_value = int(op_range.min); range.to_value = int(op_range.max); + range.step = int(op_range.step); + desc << " default value: " << int(op_range.def); crnt_descriptor.integer_range.push_back(range); if (std::is_same::value) ROS_DEBUG_STREAM("Declare: BOOL::" << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]"); @@ -119,10 +120,13 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s rcl_interfaces::msg::FloatingPointRange range; range.from_value = double(op_range.min); range.to_value = double(op_range.max); + range.step = double(op_range.step); + desc << " default value: " << double(op_range.def); crnt_descriptor.floating_point_range.push_back(range); ROS_DEBUG_STREAM("Declare: DOUBLE::" << option_name << " = " << option_value); } - + crnt_descriptor.description = desc.str(); + T new_val; try { From 8b5d6bd68ab2f6a21dff9d8ec6c4300a0d0b3aba Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 18 Jan 2023 16:00:52 +0200 Subject: [PATCH 22/85] if default profile is not defined, take the first available profile as default --- realsense2_camera/src/profile_manager.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index cb85375f73..a70fd3dc31 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -113,7 +113,10 @@ rs2::stream_profile ProfilesManager::getDefaultProfile() } } if (!(default_profile.get())) - throw std::runtime_error("No default profile found"); + { + ROS_INFO_STREAM("No default profile found. Setting the first available profile as the default one.") + default_profile = _all_profiles.front(); + } return default_profile; } From d2dacdf62d05f42b27114d9e9f4d193cdd2cfa0e Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 18 Jan 2023 16:05:50 +0200 Subject: [PATCH 23/85] fix tabs --- realsense2_camera/src/sensor_params.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index 4a23aa51b0..3dbc455d45 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -107,8 +107,8 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s rcl_interfaces::msg::IntegerRange range; range.from_value = int(op_range.min); range.to_value = int(op_range.max); - range.step = int(op_range.step); - desc << " default value: " << int(op_range.def); + range.step = int(op_range.step); + desc << " default value: " << int(op_range.def); crnt_descriptor.integer_range.push_back(range); if (std::is_same::value) ROS_DEBUG_STREAM("Declare: BOOL::" << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]"); @@ -121,7 +121,7 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s range.from_value = double(op_range.min); range.to_value = double(op_range.max); range.step = double(op_range.step); - desc << " default value: " << double(op_range.def); + desc << " default value: " << double(op_range.def); crnt_descriptor.floating_point_range.push_back(range); ROS_DEBUG_STREAM("Declare: DOUBLE::" << option_name << " = " << option_value); } From f000930509873294476de63dddb78d9dab73580f Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 18 Jan 2023 17:02:49 +0200 Subject: [PATCH 24/85] fix compilation errors --- realsense2_camera/src/profile_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index a70fd3dc31..a6f11489d6 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -114,7 +114,7 @@ rs2::stream_profile ProfilesManager::getDefaultProfile() } if (!(default_profile.get())) { - ROS_INFO_STREAM("No default profile found. Setting the first available profile as the default one.") + ROS_INFO_STREAM("No default profile found. Setting the first available profile as the default one."); default_profile = _all_profiles.front(); } From e26bb4c25397b202dd7d4d0dd63259cbc2519e26 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 24 Jan 2023 11:52:43 +0200 Subject: [PATCH 25/85] add hdr_merge.enable and depth_module.hdr_enabled to rs_launch.py --- realsense2_camera/launch/rs_launch.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 90459e2751..f5a03317dd 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -53,10 +53,12 @@ {'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'}, {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'}, {'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'}, - {'name': 'depth_module.exposure.1', 'default': '7500', 'description': 'Initial value for hdr_merge filter'}, - {'name': 'depth_module.gain.1', 'default': '16', 'description': 'Initial value for hdr_merge filter'}, - {'name': 'depth_module.exposure.2', 'default': '1', 'description': 'Initial value for hdr_merge filter'}, - {'name': 'depth_module.gain.2', 'default': '16', 'description': 'Initial value for hdr_merge filter'}, + {'name': 'depth_module.exposure.1', 'default': '7500', 'description': 'Depth module first exposure value. Used for hdr_merge filter'}, + {'name': 'depth_module.gain.1', 'default': '16', 'description': 'Depth module first gain value. Used for hdr_merge filter'}, + {'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': 'depth_module.hdr_enabled', 'default': 'false', 'description': 'Depth module hdr enablement flag. Used for hdr_merge filter'}, + {'name': 'hdr_merge.enable', 'default': 'false', 'description': 'hdr_merge filter enablement flag'}, {'name': 'wait_for_device_timeout', 'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'}, {'name': 'reconnect_timeout', 'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'}, ] From de6ea0c95e08e5bf95ddacfca0946311c64d65bc Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 24 Jan 2023 12:03:56 +0200 Subject: [PATCH 26/85] remove galactic from pre release GHA script --- .github/workflows/pre-release.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index f27237ea26..3228fa6c6c 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -31,12 +31,10 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [humble, galactic, foxy] + ros_distro: [humble, foxy] include: - ros_distro: 'humble' os: ubuntu-22.04 - - ros_distro: 'galactic' - os: ubuntu-20.04 - ros_distro: 'foxy' os: ubuntu-20.04 From 1926cd076f1aa736aa9ac0b79a24db850d5470ef Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 13 Jan 2023 16:27:45 +0100 Subject: [PATCH 27/85] make source backward compatible to older versions of cv_bridge and rclcpp --- realsense2_camera/CMakeLists.txt | 11 +++++++++++ realsense2_camera/include/base_realsense_node.h | 2 +- realsense2_camera/include/ros_param_backend.h | 2 +- 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 84de9fd5dc..a21e425a87 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -161,6 +161,17 @@ else() message(FATAL_ERROR "Unsupported ROS Distribution: " "$ENV{ROS_DISTRO}") endif() +# The header 'cv_bridge/cv_bridge.hpp' was added in version 3.3.0. For older +# cv_bridge versions, we have to use the header 'cv_bridge/cv_bridge.h'. +if(${cv_bridge_VERSION} VERSION_GREATER_EQUAL "3.3.0") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DCV_BRDIGE_HAS_HPP") +endif() + +# 'OnSetParametersCallbackType' is only defined for rclcpp 17 and onward. +if(${rclcpp_VERSION} VERSION_GREATER_EQUAL "17.0") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DRCLCPP_HAS_OnSetParametersCallbackType") +endif() + set(INCLUDES include/constants.h include/realsense_node_factory.h diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4a88f9496e..c44ba1a6b0 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -8,7 +8,7 @@ #include "constants.h" // cv_bridge.h last supported version is humble -#if defined(ROLLING) +#if defined(CV_BRDIGE_HAS_HPP) #include #else #include diff --git a/realsense2_camera/include/ros_param_backend.h b/realsense2_camera/include/ros_param_backend.h index aba3423cb8..0954cbf6d0 100644 --- a/realsense2_camera/include/ros_param_backend.h +++ b/realsense2_camera/include/ros_param_backend.h @@ -17,7 +17,7 @@ namespace realsense2_camera ~ParametersBackend(); - #if defined( ROLLING ) + #if defined( RCLCPP_HAS_OnSetParametersCallbackType ) using ros2_param_callback_type = rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; #else using ros2_param_callback_type = rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; From e4546d8257183b29342b75d544b0784543741243 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 13 Jan 2023 10:08:31 +0100 Subject: [PATCH 28/85] publish point cloud via unique shared pointer --- realsense2_camera/include/named_filter.h | 1 - realsense2_camera/src/named_filter.cpp | 48 ++++++++++++------------ 2 files changed, 25 insertions(+), 24 deletions(-) diff --git a/realsense2_camera/include/named_filter.h b/realsense2_camera/include/named_filter.h index 5a481a5f58..aeeda1af8a 100644 --- a/realsense2_camera/include/named_filter.h +++ b/realsense2_camera/include/named_filter.h @@ -54,7 +54,6 @@ namespace realsense2_camera bool _allow_no_texture_points; bool _ordered_pc; std::mutex _mutex_publisher; - sensor_msgs::msg::PointCloud2 _msg_pointcloud; rclcpp::Publisher::SharedPtr _pointcloud_publisher; std::string _pointcloud_qos; }; diff --git a/realsense2_camera/src/named_filter.cpp b/realsense2_camera/src/named_filter.cpp index 59efaa9d65..96d1a3e084 100644 --- a/realsense2_camera/src/named_filter.cpp +++ b/realsense2_camera/src/named_filter.cpp @@ -167,14 +167,16 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: rs2_intrinsics depth_intrin = pc.get_profile().as().get_intrinsics(); - sensor_msgs::PointCloud2Modifier modifier(_msg_pointcloud); + sensor_msgs::msg::PointCloud2::UniquePtr msg_pointcloud = std::make_unique(); + + sensor_msgs::PointCloud2Modifier modifier(*msg_pointcloud); modifier.setPointCloud2FieldsByString(1, "xyz"); modifier.resize(pc.size()); if (_ordered_pc) { - _msg_pointcloud.width = depth_intrin.width; - _msg_pointcloud.height = depth_intrin.height; - _msg_pointcloud.is_dense = false; + msg_pointcloud->width = depth_intrin.width; + msg_pointcloud->height = depth_intrin.height; + msg_pointcloud->is_dense = false; } vertex = pc.get_vertices(); @@ -198,14 +200,14 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: default: throw std::runtime_error("Unhandled texture format passed in pointcloud " + std::to_string(texture_frame.get_profile().format())); } - _msg_pointcloud.point_step = addPointField(_msg_pointcloud, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32, _msg_pointcloud.point_step); - _msg_pointcloud.row_step = _msg_pointcloud.width * _msg_pointcloud.point_step; - _msg_pointcloud.data.resize(_msg_pointcloud.height * _msg_pointcloud.row_step); - - sensor_msgs::PointCloud2Iteratoriter_x(_msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iteratoriter_y(_msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iteratoriter_z(_msg_pointcloud, "z"); - sensor_msgs::PointCloud2Iteratoriter_color(_msg_pointcloud, format_str); + msg_pointcloud->point_step = addPointField(*msg_pointcloud, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32, msg_pointcloud->point_step); + msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; + msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); + + sensor_msgs::PointCloud2Iteratoriter_x(*msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(*msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(*msg_pointcloud, "z"); + sensor_msgs::PointCloud2Iteratoriter_color(*msg_pointcloud, format_str); color_point = pc.get_texture_coordinates(); float color_pixel[2]; @@ -239,12 +241,12 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: else { std::string format_str = "intensity"; - _msg_pointcloud.row_step = _msg_pointcloud.width * _msg_pointcloud.point_step; - _msg_pointcloud.data.resize(_msg_pointcloud.height * _msg_pointcloud.row_step); + msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; + msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); - sensor_msgs::PointCloud2Iteratoriter_x(_msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iteratoriter_y(_msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iteratoriter_z(_msg_pointcloud, "z"); + sensor_msgs::PointCloud2Iteratoriter_x(*msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(*msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(*msg_pointcloud, "z"); for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++) { @@ -260,19 +262,19 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: } } } - _msg_pointcloud.header.stamp = t; - _msg_pointcloud.header.frame_id = frame_id; + msg_pointcloud->header.stamp = t; + msg_pointcloud->header.frame_id = frame_id; if (!_ordered_pc) { - _msg_pointcloud.width = valid_count; - _msg_pointcloud.height = 1; - _msg_pointcloud.is_dense = true; + msg_pointcloud->width = valid_count; + msg_pointcloud->height = 1; + msg_pointcloud->is_dense = true; modifier.resize(valid_count); } { std::lock_guard lock_guard(_mutex_publisher); if (_pointcloud_publisher) - _pointcloud_publisher->publish(_msg_pointcloud); + _pointcloud_publisher->publish(std::move(msg_pointcloud)); } } From 2ff18513092fbdb5af2a4945fafd7272425cf042 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 13 Jan 2023 11:26:11 +0100 Subject: [PATCH 29/85] remove unused variable format_str --- realsense2_camera/src/named_filter.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/realsense2_camera/src/named_filter.cpp b/realsense2_camera/src/named_filter.cpp index 96d1a3e084..35871c0c0f 100644 --- a/realsense2_camera/src/named_filter.cpp +++ b/realsense2_camera/src/named_filter.cpp @@ -240,7 +240,6 @@ void PointcloudFilter::Publish(rs2::points pc, const rclcpp::Time& t, const rs2: } else { - std::string format_str = "intensity"; msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); From a2026cf854f7c0e26d9b371ba4bf8bc82ae7bec6 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Fri, 13 Jan 2023 16:36:01 +0100 Subject: [PATCH 30/85] remove extra ';' --- realsense2_camera/include/ros_param_backend.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/include/ros_param_backend.h b/realsense2_camera/include/ros_param_backend.h index 0954cbf6d0..a68495fee2 100644 --- a/realsense2_camera/include/ros_param_backend.h +++ b/realsense2_camera/include/ros_param_backend.h @@ -13,7 +13,7 @@ namespace realsense2_camera ParametersBackend(rclcpp::Node& node) : _node(node), _logger(node.get_logger()) - {}; + {} ~ParametersBackend(); From c9fc09b4294399d159fcc471eea7e15b82ea24bb Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 25 Jan 2023 02:14:14 +0200 Subject: [PATCH 31/85] changed to static_cast and added descriptor name and type --- realsense2_camera/src/sensor_params.cpp | 30 +++++++++++++++++-------- 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index 3dbc455d45..12c179230d 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -100,31 +100,43 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s } rs2::option_range op_range = sensor.get_option_range(option); rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; + + // set descriptor name to be the option name in order to get more detailed warnings/errors + crnt_descriptor.name = option_name; + std::stringstream desc; desc << sensor.get_option_description(option) << std::endl << description_addition; if (std::is_same::value || std::is_same::value) { rcl_interfaces::msg::IntegerRange range; - range.from_value = int(op_range.min); - range.to_value = int(op_range.max); - range.step = int(op_range.step); - desc << " default value: " << int(op_range.def); + range.from_value = static_cast(op_range.min); + range.to_value = static_cast(op_range.max); + range.step = static_cast(op_range.step); + desc << " default value: " << static_cast(op_range.def); crnt_descriptor.integer_range.push_back(range); - if (std::is_same::value) + if (std::is_same::value) + { + crnt_descriptor.type = rclcpp::PARAMETER_BOOL; ROS_DEBUG_STREAM("Declare: BOOL::" << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]"); + } else + { + crnt_descriptor.type = rclcpp::PARAMETER_INTEGER; ROS_DEBUG_STREAM("Declare: INT::" << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]"); + } } else { rcl_interfaces::msg::FloatingPointRange range; - range.from_value = double(op_range.min); - range.to_value = double(op_range.max); - range.step = double(op_range.step); - desc << " default value: " << double(op_range.def); + range.from_value = static_cast(op_range.min); + range.to_value = static_cast(op_range.max); + range.step = static_cast(op_range.step); + desc << " default value: " << static_cast(op_range.def); crnt_descriptor.floating_point_range.push_back(range); + crnt_descriptor.type = rclcpp::PARAMETER_DOUBLE; ROS_DEBUG_STREAM("Declare: DOUBLE::" << option_name << " = " << option_value); } + crnt_descriptor.description = desc.str(); T new_val; From da04a3a9dc01058051261ff5e0b27960af98ae8d Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 29 Jan 2023 04:37:10 +0200 Subject: [PATCH 32/85] remove T265 device from ROS Wrapper - step1 --- README.md | 24 +--- realsense2_camera/CMakeLists.txt | 2 - realsense2_camera/include/constants.h | 1 - .../include/t265_realsense_node.h | 32 ----- .../launch/rs_d400_and_t265_launch.py | 4 - realsense2_camera/launch/rs_launch.py | 1 - realsense2_camera/package.xml | 2 +- .../src/realsense_node_factory.cpp | 4 - realsense2_camera/src/t265_realsense_node.cpp | 127 ------------------ 9 files changed, 6 insertions(+), 191 deletions(-) delete mode 100644 realsense2_camera/include/t265_realsense_node.h delete mode 100644 realsense2_camera/src/t265_realsense_node.cpp diff --git a/README.md b/README.md index 6f78cf632f..946cf8de46 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # ROS2 Wrapper for Intel® RealSense™ Devices -These are packages for using Intel RealSense cameras (D400 and L500 series, SR300 camera and T265 Tracking Module) with ROS2. +These are packages for using Intel RealSense cameras (D400, L500 and SR300 cameras) with ROS2. This version supports ROS2 Dashing, Eloquent, Foxy, Galactic and Rolling. @@ -157,7 +157,7 @@ Enabling stream adds matching topics. For instance, enabling the gyro and accel To turn them off: `ros2 param set /camera/camera enable_infra false` The "/camera" prefix is the namesapce specified in the given launch file. -When using D435 or D415, the gyro and accel topics wont be available. Likewise, other topics will be available when using T265 (see below). +When using D435 or D415, the gyro and accel topics wont be available. ### The metadata topic: The metadata messages store the camera's available metadata in a *json* format. To learn more, a dedicated script for echoing a metadata topic in runtime is attached. For instance, use the following command to echo the camera/depth/metadata topic: @@ -230,14 +230,14 @@ For setting a new value for a parameter use `ros2 param set None, 1 -> Copy, 2 -> Linear_ interpolation] when: - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. - **copy**: Every gyro message is attached by the last accel message. - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) -- **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value. +- **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. -- **topic_odom_in**: For T265, add wheel odometry information through this topic. The code refers only to the *twist.linear* field in the message. +- **topic_odom_in**: The code refers only to the *twist.linear* field in the message. - **calib_odom_file**: For the T265 to include odometry input, it must be given a [configuration file](https://github.com/IntelRealSense/librealsense/blob/master/unit-tests/resources/calibration_odometry.json). Explanations can be found [here](https://github.com/IntelRealSense/librealsense/pull/3462). The calibration is done in ROS coordinates system. - **publish_tf**: boolean, publish or not TF at all. Defaults to True. - **diagnostics_period**: double, positive values set the period between diagnostics updates on the `/diagnostics` topic. 0 or negative values mean no diagnostics topic is published. Defaults to 0.
@@ -248,20 +248,6 @@ The `/diagnostics` topic includes information regarding the device temperatures ### Available services: - device_info : retrieve information about the device - serial_number, firmware_version etc. Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. Call example: `ros2 service call /camera/device_info realsense2_camera_msgs/srv/DeviceInfo` - Note that for **ROS2 Dashing** the command is `ros2 srv show realsense2_camera_msgs/srv/DeviceInfo` -## Using T265 ## - -### Start the camera node -To start the camera node: - -```bash -ros2 run realsense2_camera realsense2_camera_node --ros-args -p enable_pose:=true -p device_type:=t265 -``` -or, if you also have a d4xx connected, you can try out the launch file: -```bash -ros2 launch realsense2_camera rs_d400_and_t265_launch.py enable_fisheye12:=true enable_fisheye22:=true -``` -- note: the parameters are called `enable_fisheye12` and `enable_fisheye22`. The node knows them as `enable_fisheye1` and `enable_fisheye2` but launch file runs 2 nodes and these parameters refer to the second one. - ## Efficient intra-process communication: diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 84de9fd5dc..c6f7f45bb2 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -118,7 +118,6 @@ set(node_plugins "") set(SOURCES src/realsense_node_factory.cpp src/base_realsense_node.cpp - src/t265_realsense_node.cpp src/parameters.cpp src/rs_node_setup.cpp src/ros_sensor.cpp @@ -165,7 +164,6 @@ set(INCLUDES include/constants.h include/realsense_node_factory.h include/base_realsense_node.h - include/t265_realsense_node.h include/ros_sensor.h include/ros_utils.h include/dynamic_params.h diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index f640bab854..1c1961f827 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -68,7 +68,6 @@ namespace realsense2_camera const uint16_t RS416_RGB_PID = 0x0B52; // F416 RGB const uint16_t RS405_PID = 0x0B5B; // DS5U const uint16_t RS455_PID = 0x0B5C; // D455 - const uint16_t RS_T265_PID = 0x0b37; // const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // const uint16_t RS_L515_PID = 0x0B64; // const uint16_t RS_L535_PID = 0x0b68; diff --git a/realsense2_camera/include/t265_realsense_node.h b/realsense2_camera/include/t265_realsense_node.h deleted file mode 100644 index c8e1f312ca..0000000000 --- a/realsense2_camera/include/t265_realsense_node.h +++ /dev/null @@ -1,32 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. - -#pragma once - -#include -#include "std_msgs/msg/string.hpp" - -namespace realsense2_camera -{ - class T265RealsenseNode : public BaseRealSenseNode - { - public: - T265RealsenseNode(rclcpp::Node& node, - rs2::device dev, - std::shared_ptr parameters, - bool use_intra_process = false); - void publishTopics(); - - protected: - void calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile) override; - - private: - void initializeOdometryInput(); - void setupSubscribers(); - void odom_in_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - - rclcpp::Subscription::SharedPtr _odom_subscriber; - rs2::wheel_odometer _wo_snr; - bool _use_odom_in; - }; -} diff --git a/realsense2_camera/launch/rs_d400_and_t265_launch.py b/realsense2_camera/launch/rs_d400_and_t265_launch.py index 54ea3f927b..a62731affe 100644 --- a/realsense2_camera/launch/rs_d400_and_t265_launch.py +++ b/realsense2_camera/launch/rs_d400_and_t265_launch.py @@ -13,10 +13,6 @@ local_parameters = [{'name': 'camera_name1', 'default': 'D400', 'description': 'camera unique name'}, {'name': 'device_type1', 'default': 'd4.', 'description': 'choose device by type'}, - {'name': 'camera_name2', 'default': 'T265', 'description': 'camera unique name'}, - {'name': 'device_type2', 'default': 't265', 'description': 'choose device by type'}, - {'name': 'enable_fisheye12', 'default': 'false', 'description': 'topic for T265 wheel odometry'}, - {'name': 'enable_fisheye22', 'default': 'false', 'description': 'topic for T265 wheel odometry'}, ] def generate_launch_description(): diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index f5a03317dd..79dadb0e52 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -48,7 +48,6 @@ {'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"}, {'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''}, {'name': 'calib_odom_file', 'default': "''", 'description': "''"}, - {'name': 'topic_odom_in', 'default': "''", 'description': 'topic for T265 wheel odometry'}, {'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'}, {'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'}, {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'}, diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index b5cedbe649..0556f105ba 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -3,7 +3,7 @@ realsense2_camera 4.51.1 - RealSense camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras + RealSense camera package allowing access to Intel SR300 and D400 3D cameras LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 9c071b366c..b356b501db 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -3,7 +3,6 @@ #include "../include/realsense_node_factory.h" #include "../include/base_realsense_node.h" -#include "../include/t265_realsense_node.h" #include #include #include @@ -380,9 +379,6 @@ void RealSenseNodeFactory::startDevice() case RS_L535_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); break; - case RS_T265_PID: - _realSenseNode = std::unique_ptr(new T265RealsenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms())); - break; default: ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); rclcpp::shutdown(); diff --git a/realsense2_camera/src/t265_realsense_node.cpp b/realsense2_camera/src/t265_realsense_node.cpp deleted file mode 100644 index 523363282d..0000000000 --- a/realsense2_camera/src/t265_realsense_node.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. - -#include "../include/t265_realsense_node.h" -#include - -using namespace realsense2_camera; - -T265RealsenseNode::T265RealsenseNode(rclcpp::Node& node, - rs2::device dev, - std::shared_ptr parameters, - bool use_intra_process) : - BaseRealSenseNode(node, dev, parameters, use_intra_process), - _wo_snr(dev.first()), - _use_odom_in(false) - { - _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_MOTION_MODULE_TEMPERATURE}; - initializeOdometryInput(); - } - -void T265RealsenseNode::initializeOdometryInput() -{ - std::string calib_odom_file; - std::string param_name("calib_odom_file"); - calib_odom_file = _parameters->setParam(param_name, ""); - if (calib_odom_file.empty()) - { - ROS_INFO("No calib_odom_file. No input odometry accepted."); - return; - } - _parameters_names.push_back(param_name); - - std::ifstream calibrationFile(calib_odom_file); - if (!calibrationFile) - { - ROS_FATAL_STREAM("calibration_odometry file not found. calib_odom_file = " << calib_odom_file); - throw std::runtime_error("calibration_odometry file not found" ); - } - const std::string json_str((std::istreambuf_iterator(calibrationFile)), - std::istreambuf_iterator()); - const std::vector wo_calib(json_str.begin(), json_str.end()); - - if (!_wo_snr.load_wheel_odometery_config(wo_calib)) - { - ROS_FATAL_STREAM("Format error in calibration_odometry file: " << calib_odom_file); - throw std::runtime_error("Format error in calibration_odometry file" ); - } - _use_odom_in = true; -} - -void T265RealsenseNode::publishTopics() -{ - BaseRealSenseNode::publishTopics(); - setupSubscribers(); -} - -void T265RealsenseNode::setupSubscribers() -{ - if (!_use_odom_in) return; - - std::string topic_odom_in = _parameters->setParam("topic_odom_in", DEFAULT_TOPIC_ODOM_IN); - ROS_INFO_STREAM("Subscribing to in_odom topic: " << topic_odom_in); - - _odom_subscriber = _node.create_subscription(topic_odom_in, 1, [this](const nav_msgs::msg::Odometry::SharedPtr msg){odom_in_callback(msg);}); -} - -void T265RealsenseNode::odom_in_callback(const nav_msgs::msg::Odometry::SharedPtr msg) -{ - ROS_DEBUG("Got in_odom message"); - rs2_vector velocity {-(float)(msg->twist.twist.linear.y), - (float)(msg->twist.twist.linear.z), - -(float)(msg->twist.twist.linear.x)}; - - ROS_DEBUG_STREAM("Add odom: " << velocity.x << ", " << velocity.y << ", " << velocity.z); - _wo_snr.send_wheel_odometry(0, 0, velocity); -} - -void T265RealsenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile) -{ - // Transform base to stream - stream_index_pair sip(profile.stream_type(), profile.stream_index()); - - tf2::Quaternion quaternion_optical; - quaternion_optical.setRPY(M_PI / 2, 0.0, -M_PI / 2); //Pose To ROS - float3 zero_trans{0, 0, 0}; - - rclcpp::Time transform_ts_ = _node.now(); - - rs2_extrinsics ex; - try - { - ex = base_profile.get_extrinsics_to(profile); - } - catch (std::exception& e) - { - if (!strcmp(e.what(), "Requested extrinsics are not available!")) - { - ROS_WARN_STREAM("(" << rs2_stream_to_string(profile.stream_type()) << ", " << profile.stream_index() << ") -> (" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << "): " << e.what() << " : using unity as default."); - ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); - } - else - { - throw e; - } - } - - auto Q = rotationMatrixToQuaternion(ex.rotation); - Q = quaternion_optical * Q * quaternion_optical.inverse(); - float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]}; - - if (sip == POSE) - { - Q = Q.inverse(); - publish_static_tf(transform_ts_, trans, Q, FRAME_ID(sip), _base_frame_id); - } - else - { - publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); - - if (profile.is() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1) - { - publish_static_tf(transform_ts_, trans, Q, _base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); - } - } -} From 1a86b0d4724ce92006cd038197cb29e81dbe96c5 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 30 Jan 2023 05:51:33 +0200 Subject: [PATCH 33/85] realsense2_camera/src/sensor_params.cpp --- realsense2_camera/src/sensor_params.cpp | 28 ++++++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index 12c179230d..68ab6da957 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -89,16 +89,31 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s { const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(option))); T option_value; + rs2::option_range op_range; try { - option_value = static_cast(sensor.get_option(option)); + op_range = sensor.get_option_range(option); + float current_val = sensor.get_option(option); + if(std::is_same::value && current_val > 0) + { + // casting from float to double, while trying to increase precision + // in order to be comply with the FloatingPointRange configurations + // and to succeed the rclcpp NodeParameters::declare_parameter call + float range = op_range.max - op_range.min; + int temp_val = range / current_val; + option_value = static_cast(range) / temp_val; + } + else + { + option_value = static_cast(current_val); + } } catch(const std::exception& ex) { ROS_ERROR_STREAM("An error has occurred while calling sensor for: " << option_name << ":" << ex.what()); return; } - rs2::option_range op_range = sensor.get_option_range(option); + rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; // set descriptor name to be the option name in order to get more detailed warnings/errors @@ -130,7 +145,14 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s rcl_interfaces::msg::FloatingPointRange range; range.from_value = static_cast(op_range.min); range.to_value = static_cast(op_range.max); - range.step = static_cast(op_range.step); + + // casting from float to double, while trying to increase precision + // in order to be comply with the FloatingPointRange configurations + // and to succeed the rclcpp NodeParameters::declare_parameter call + float r = op_range.max - op_range.min; + int steps = r / op_range.step; + range.step = static_cast(r) / steps; + desc << " default value: " << static_cast(op_range.def); crnt_descriptor.floating_point_range.push_back(range); crnt_descriptor.type = rclcpp::PARAMETER_DOUBLE; From 675eb5187ec686204d3da9605c345fdbb9db1f7a Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 31 Jan 2023 02:45:27 +0200 Subject: [PATCH 34/85] fix code review and split big function into smaller functions --- realsense2_camera/include/sensor_params.h | 4 + realsense2_camera/src/sensor_params.cpp | 154 ++++++++++++++-------- 2 files changed, 100 insertions(+), 58 deletions(-) diff --git a/realsense2_camera/include/sensor_params.h b/realsense2_camera/include/sensor_params.h index 878efbd3fb..ea8ba5fb37 100644 --- a/realsense2_camera/include/sensor_params.h +++ b/realsense2_camera/include/sensor_params.h @@ -23,6 +23,10 @@ namespace realsense2_camera rclcpp::Logger _logger; private: + double float_to_double(float val, rs2::option_range option_range); + template + rcl_interfaces::msg::ParameterDescriptor get_parameter_descriptor(const std::string& option_name, rs2::option_range option_range, + T option_value, const std::string& option_description, const std::string& description_addition); template void set_parameter(rs2::options sensor, rs2_option option, const std::string& module_name, const std::string& description_addition=""); diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index 68ab6da957..a6ed3be472 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -84,82 +84,120 @@ SensorParams::~SensorParams() clearParameters(); } +double SensorParams::float_to_double(float val, rs2::option_range option_range) +{ + if(val == 0) + { + return 0.0; + } + + // casting from float to double, while trying to increase precision + // in order to be comply with the FloatingPointRange configurations + // and to succeed the rclcpp NodeParameters::declare_parameter call + // for more info, see https://github.com/ros2/rclcpp/blob/rolling/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp + // rcl_interfaces::msg::SetParametersResult and __are_doubles_equal methods + float range = option_range.max - option_range.min; + int steps = range / option_range.step; + double step = static_cast(range) / steps; + double rounded_div = std::round((val - option_range.min) / step); + return option_range.min + rounded_div * step; +} + +//for more info, see https://docs.ros2.org/galactic/api/rcl_interfaces/msg/ParameterDescriptor.html template -void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const std::string& module_name, const std::string& description_addition) +rcl_interfaces::msg::ParameterDescriptor SensorParams::get_parameter_descriptor(const std::string& option_name, + rs2::option_range option_range, T option_value, const std::string& option_description, const std::string& description_addition) { - const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(option))); - T option_value; - rs2::option_range op_range; - try + rcl_interfaces::msg::ParameterDescriptor parameter_descriptor; + + // set descriptor name to be the option name in order to get more detailed warnings/errors from rclcpp + parameter_descriptor.name = option_name; + + // create description stream, and start filling it. + std::stringstream description_stream; + description_stream << option_description << std::endl << description_addition; + if(description_addition.length() > 0) + description_stream << std::endl; + + if (std::is_same::value) { - op_range = sensor.get_option_range(option); - float current_val = sensor.get_option(option); - if(std::is_same::value && current_val > 0) + parameter_descriptor.type = rclcpp::PARAMETER_DOUBLE; + + // set option range values (floats) + rcl_interfaces::msg::FloatingPointRange range; + range.from_value = float_to_double(option_range.min, option_range); + range.to_value = float_to_double(option_range.max, option_range); + range.step = float_to_double(option_range.step, option_range); + parameter_descriptor.floating_point_range.push_back(range); + + // add the default value of this option to the description stream + description_stream << "Default value: " << static_cast(option_range.def); + + ROS_INFO_STREAM("Declare: DOUBLE::" << option_name << "=" << option_value << " [" << option_range.min << ", " << option_range.max << "]"); + } + else // T is bool or int + { + // set option range values (integers) (suitable for boolean ranges also: [0,1]) + rcl_interfaces::msg::IntegerRange range; + range.from_value = static_cast(option_range.min); + range.to_value = static_cast(option_range.max); + range.step = static_cast(option_range.step); + parameter_descriptor.integer_range.push_back(range); + + if (std::is_same::value) { - // casting from float to double, while trying to increase precision - // in order to be comply with the FloatingPointRange configurations - // and to succeed the rclcpp NodeParameters::declare_parameter call - float range = op_range.max - op_range.min; - int temp_val = range / current_val; - option_value = static_cast(range) / temp_val; + parameter_descriptor.type = rclcpp::PARAMETER_BOOL; + + // add the default value of this option to the description stream + description_stream << "Default value: " << (option_range.def == 0 ? "False" : "True"); + + ROS_DEBUG_STREAM("Declare: BOOL::" << option_name << "=" << (option_range.def == 0 ? "False" : "True") << " [" << option_range.min << ", " << option_range.max << "]"); } else { - option_value = static_cast(current_val); + parameter_descriptor.type = rclcpp::PARAMETER_INTEGER; + + // add the default value of this option to the description stream + description_stream << "Default value: " << static_cast(option_range.def); + + ROS_DEBUG_STREAM("Declare: INT::" << option_name << "=" << option_value << " [" << option_range.min << ", " << option_range.max << "]"); } } - catch(const std::exception& ex) - { - ROS_ERROR_STREAM("An error has occurred while calling sensor for: " << option_name << ":" << ex.what()); - return; - } - - rcl_interfaces::msg::ParameterDescriptor crnt_descriptor; + parameter_descriptor.description = description_stream.str(); + return parameter_descriptor; +} - // set descriptor name to be the option name in order to get more detailed warnings/errors - crnt_descriptor.name = option_name; +template +void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const std::string& module_name, const std::string& description_addition) +{ + // set the option name, for example: depth_module.exposure + const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(option))); - std::stringstream desc; - desc << sensor.get_option_description(option) << std::endl << description_addition; - if (std::is_same::value || std::is_same::value) + // get option current value, and option range values from the sensor + T option_value; + rs2::option_range option_range; + try { - rcl_interfaces::msg::IntegerRange range; - range.from_value = static_cast(op_range.min); - range.to_value = static_cast(op_range.max); - range.step = static_cast(op_range.step); - desc << " default value: " << static_cast(op_range.def); - crnt_descriptor.integer_range.push_back(range); - if (std::is_same::value) + option_range = sensor.get_option_range(option); + float current_val = sensor.get_option(option); + if(std::is_same::value) { - crnt_descriptor.type = rclcpp::PARAMETER_BOOL; - ROS_DEBUG_STREAM("Declare: BOOL::" << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]"); + option_value = float_to_double(current_val, option_range); } else { - crnt_descriptor.type = rclcpp::PARAMETER_INTEGER; - ROS_DEBUG_STREAM("Declare: INT::" << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]"); + option_value = static_cast(current_val); } } - else + catch(const std::exception& ex) { - rcl_interfaces::msg::FloatingPointRange range; - range.from_value = static_cast(op_range.min); - range.to_value = static_cast(op_range.max); - - // casting from float to double, while trying to increase precision - // in order to be comply with the FloatingPointRange configurations - // and to succeed the rclcpp NodeParameters::declare_parameter call - float r = op_range.max - op_range.min; - int steps = r / op_range.step; - range.step = static_cast(r) / steps; - - desc << " default value: " << static_cast(op_range.def); - crnt_descriptor.floating_point_range.push_back(range); - crnt_descriptor.type = rclcpp::PARAMETER_DOUBLE; - ROS_DEBUG_STREAM("Declare: DOUBLE::" << option_name << " = " << option_value); + ROS_ERROR_STREAM("An error has occurred while calling sensor for: " << option_name << ":" << ex.what()); + return; } - crnt_descriptor.description = desc.str(); + // get parameter descriptor for this option + rcl_interfaces::msg::ParameterDescriptor parameter_descriptor = + get_parameter_descriptor(option_name, option_range, option_value, sensor.get_option_description(option), description_addition); T new_val; try @@ -167,12 +205,12 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s new_val = (_parameters->setParam(option_name, option_value, [option, sensor](const rclcpp::Parameter& parameter) { param_set_option(sensor, option, parameter); - }, crnt_descriptor)); + }, parameter_descriptor)); _parameters_names.push_back(option_name); } catch(const rclcpp::exceptions::InvalidParameterValueException& e) { - ROS_WARN_STREAM("Failed to set parameter:" << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]\n" << e.what()); + ROS_WARN_STREAM("Failed to set parameter:" << option_name << " = " << option_value << "[" << option_range.min << ", " << option_range.max << "]\n" << e.what()); return; } @@ -184,7 +222,7 @@ void SensorParams::set_parameter(rs2::options sensor, rs2_option option, const s } catch(std::exception& e) { - ROS_WARN_STREAM("Failed to set value to sensor: " << option_name << " = " << option_value << "[" << op_range.min << ", " << op_range.max << "]\n" << e.what()); + ROS_WARN_STREAM("Failed to set value to sensor: " << option_name << " = " << option_value << "[" << option_range.min << ", " << option_range.max << "]\n" << e.what()); } } } From 3e711ad9abe670236bd6c233749529500fb06bdc Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 31 Jan 2023 03:01:43 +0200 Subject: [PATCH 35/85] fix float_to_double method --- realsense2_camera/src/sensor_params.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index a6ed3be472..b6e8a7fb3e 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -86,11 +86,6 @@ SensorParams::~SensorParams() double SensorParams::float_to_double(float val, rs2::option_range option_range) { - if(val == 0) - { - return 0.0; - } - // casting from float to double, while trying to increase precision // in order to be comply with the FloatingPointRange configurations // and to succeed the rclcpp NodeParameters::declare_parameter call @@ -133,7 +128,7 @@ rcl_interfaces::msg::ParameterDescriptor SensorParams::get_parameter_descriptor( // add the default value of this option to the description stream description_stream << "Default value: " << static_cast(option_range.def); - ROS_INFO_STREAM("Declare: DOUBLE::" << option_name << "=" << option_value << " [" << option_range.min << ", " << option_range.max << "]"); + ROS_DEBUG_STREAM("Declare: DOUBLE::" << option_name << "=" << option_value << " [" << option_range.min << ", " << option_range.max << "]"); } else // T is bool or int { From ec1d83e3f7c961a6a1692cbb2d207e23d60c0f59 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 31 Jan 2023 12:32:01 +0200 Subject: [PATCH 36/85] remove T265 - edit launch files and README --- README.md | 2 -- realsense2_camera/include/constants.h | 1 - .../launch/rs_d400_and_t265_launch.py | 26 ------------------- realsense2_camera/launch/rs_launch.py | 1 - 4 files changed, 30 deletions(-) delete mode 100644 realsense2_camera/launch/rs_d400_and_t265_launch.py diff --git a/README.md b/README.md index 946cf8de46..6659fdb695 100644 --- a/README.md +++ b/README.md @@ -237,8 +237,6 @@ Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. -- **topic_odom_in**: The code refers only to the *twist.linear* field in the message. -- **calib_odom_file**: For the T265 to include odometry input, it must be given a [configuration file](https://github.com/IntelRealSense/librealsense/blob/master/unit-tests/resources/calibration_odometry.json). Explanations can be found [here](https://github.com/IntelRealSense/librealsense/pull/3462). The calibration is done in ROS coordinates system. - **publish_tf**: boolean, publish or not TF at all. Defaults to True. - **diagnostics_period**: double, positive values set the period between diagnostics updates on the `/diagnostics` topic. 0 or negative values mean no diagnostics topic is published. Defaults to 0.
The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 1c1961f827..387ffd3515 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -102,7 +102,6 @@ namespace realsense2_camera const std::string DEFAULT_UNITE_IMU_METHOD = ""; const std::string DEFAULT_FILTERS = ""; - const std::string DEFAULT_TOPIC_ODOM_IN = ""; const float ROS_DEPTH_SCALE = 0.001; diff --git a/realsense2_camera/launch/rs_d400_and_t265_launch.py b/realsense2_camera/launch/rs_d400_and_t265_launch.py deleted file mode 100644 index a62731affe..0000000000 --- a/realsense2_camera/launch/rs_d400_and_t265_launch.py +++ /dev/null @@ -1,26 +0,0 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. - -"""Launch realsense2_camera node without rviz2.""" -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import 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': 'D400', 'description': 'camera unique name'}, - {'name': 'device_type1', 'default': 'd4.', 'description': 'choose device by type'}, - ] - -def generate_launch_description(): - return LaunchDescription( - rs_launch.declare_configurable_parameters(local_parameters) + - [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_multi_camera_launch.py']), - launch_arguments=rs_launch.set_configurable_parameters(local_parameters).items(), - ), - ]) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 79dadb0e52..e2e5bbc24d 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -47,7 +47,6 @@ {'name': 'initial_reset', 'default': 'false', 'description': "''"}, {'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"}, {'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''}, - {'name': 'calib_odom_file', 'default': "''", 'description': "''"}, {'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'}, {'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'}, {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'}, From 80d9bb707d0f0ac3778aa119690c266f3a87a191 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 31 Jan 2023 14:06:19 +0200 Subject: [PATCH 37/85] remove T265 leftovers --- realsense2_camera/launch/rs_launch.py | 1 - realsense2_camera/src/realsense_node_factory.cpp | 6 ------ 2 files changed, 7 deletions(-) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index e2e5bbc24d..fd0310f9a6 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -26,7 +26,6 @@ {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, {'name': 'infra_rgb', 'default': 'false', 'description': 'enable infra2 stream'}, - {'name': 'tracking_module.profile', 'default': '0,0,0', 'description': 'fisheye width'}, {'name': 'enable_fisheye1', 'default': 'true', 'description': 'enable fisheye1 stream'}, {'name': 'enable_fisheye2', 'default': 'true', 'description': 'enable fisheye2 stream'}, {'name': 'enable_confidence', 'default': 'true', 'description': 'enable depth stream'}, diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 51ea0f8b37..b5f68ef2b2 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -174,12 +174,6 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) } } - bool remove_tm2_handle(_device && RS_T265_PID != std::stoi(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID), 0, 16)); - if (remove_tm2_handle) - { - _ctx.unload_tracking_module(); - } - if (_device && _initial_reset) { _initial_reset = false; From 492e9c7757eb343439b0c6e819931dec36afdf07 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 1 Feb 2023 22:32:35 +0200 Subject: [PATCH 38/85] apply copyrights and license on project --- .github/workflows/main.yml | 22 +++---- CONTRIBUTING.md | 56 ++++++++++++++++ LICENSE | 2 +- realsense2_camera/CMakeLists.txt | 15 ++++- .../include/base_realsense_node.h | 15 ++++- realsense2_camera/include/constants.h | 15 ++++- realsense2_camera/include/dynamic_params.h | 15 ++++- realsense2_camera/include/image_publisher.h | 15 ++++- realsense2_camera/include/named_filter.h | 15 ++++- realsense2_camera/include/profile_manager.h | 15 ++++- .../include/realsense_node_factory.h | 15 ++++- realsense2_camera/include/ros_param_backend.h | 15 ++++- realsense2_camera/include/ros_sensor.h | 15 ++++- realsense2_camera/include/ros_utils.h | 15 ++++- realsense2_camera/include/sensor_params.h | 15 ++++- .../launch/rs_intra_process_demo_launch.py | 15 ++++- realsense2_camera/launch/rs_launch.py | 15 ++++- .../launch/rs_multi_camera_launch.py | 16 ++++- realsense2_camera/scripts/echo_metadada.py | 15 ++++- realsense2_camera/scripts/rs2_listener.py | 15 ++++- realsense2_camera/scripts/rs2_test.py | 15 ++++- .../scripts/set_cams_transforms.py | 15 ++++- .../scripts/show_center_depth.py | 15 ++++- realsense2_camera/scripts/topic_hz.py | 15 ++++- realsense2_camera/src/base_realsense_node.cpp | 15 ++++- realsense2_camera/src/dynamic_params.cpp | 15 ++++- realsense2_camera/src/image_publisher.cpp | 15 ++++- realsense2_camera/src/named_filter.cpp | 15 ++++- realsense2_camera/src/parameters.cpp | 15 ++++- realsense2_camera/src/profile_manager.cpp | 15 ++++- .../src/realsense_node_factory.cpp | 15 ++++- .../src/ros_param_backend_dashing.cpp | 15 ++++- .../src/ros_param_backend_foxy.cpp | 15 ++++- .../src/ros_param_backend_rolling.cpp | 15 ++++- realsense2_camera/src/ros_sensor.cpp | 15 ++++- realsense2_camera/src/ros_utils.cpp | 15 ++++- realsense2_camera/src/rs_node_setup.cpp | 15 ++++- realsense2_camera/src/sensor_params.cpp | 15 ++++- .../tools/frame_latency/frame_latency.cpp | 15 ++++- .../tools/frame_latency/frame_latency.h | 15 ++++- realsense2_camera_msgs/CMakeLists.txt | 15 ++++- realsense2_description/CMakeLists.txt | 15 ++++- realsense2_description/launch/launch_utils.py | 15 ++++- .../launch/view_model.launch.py | 15 ++++- realsense2_description/urdf/_d415.urdf.xacro | 17 ++++- realsense2_description/urdf/_d435.urdf.xacro | 17 ++++- realsense2_description/urdf/_d435i.urdf.xacro | 18 ++++- .../urdf/_d435i_imu_modules.urdf.xacro | 17 ++++- realsense2_description/urdf/_d455.urdf.xacro | 17 ++++- realsense2_description/urdf/_l515.urdf.xacro | 17 ++++- .../urdf/_materials.urdf.xacro | 17 ++++- realsense2_description/urdf/_r410.urdf.xacro | 17 ++++- realsense2_description/urdf/_r430.urdf.xacro | 17 ++++- .../urdf/_usb_plug.urdf.xacro | 17 ++++- scripts/pr_check.sh | 65 +++++++++---------- 55 files changed, 784 insertions(+), 148 deletions(-) create mode 100644 CONTRIBUTING.md diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index a0777f17fb..695909a32f 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -40,15 +40,6 @@ jobs: steps: - # Workaround - forcing cmake 3.23.0 instead of using latest cmake of GHA. - # cmake 3.24.0 is not compaitable with galactic/foxy/humble ament_cmake - # see: https://github.com/ament/ament_cmake/pull/395 - # TODO: remove this step once ament_cmake for galactic/foxy/humble is released - - name: Setup cmake 3.23.0 - uses: jwlawson/actions-setup-cmake@v1.12.1 - with: - cmake-version: '3.23.0' - - name: Setup ROS2 Workspace run: | mkdir -p ${{github.workspace}}/ros2/src @@ -62,7 +53,16 @@ jobs: run: | cd ${{github.workspace}}/ros2/src/realsense-ros/scripts ./pr_check.sh - + + # Workaround - forcing cmake 3.23.0 instead of using latest cmake of GHA. + # cmake 3.24.0 is not compaitable with galactic/foxy/humble ament_cmake + # see: https://github.com/ament/ament_cmake/pull/395 + # TODO: remove this step once ament_cmake for galactic/foxy/humble is released + - name: Setup cmake 3.23.0 + uses: jwlawson/actions-setup-cmake@v1.12.1 + with: + cmake-version: '3.23.0' + ## If distro is one of [galactic, foxy, eloquent, dashing], use the setup-ros@v0.2 ## which supports old versions of ROS2 and EOL versions. ## For Humble distro, use v0.3 @@ -140,4 +140,4 @@ jobs: source ${{github.workspace}}/.bashrc . install/local_setup.bash python3 src/realsense-ros/realsense2_camera/scripts/rs2_test.py non_existent_file - \ No newline at end of file + diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000000..3632ad8983 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,56 @@ +# How to Contribute + +This project welcomes third-party code via GitHub pull requests. + +You are welcome to propose and discuss enhancements using project [issues](https://github.com/IntelRealSense/realsense-ros/issues). + +> **Branching Policy**: +> The `ros2-development` branch is considered stable, at all times. +> If you plan to propose a patch, please commit into the `ros2-development` branch, or its own feature branch. + +In addition, please run `pr_check.sh` under `scripts` directory. This scripts verify compliance with project's standards: + +1. Every example / source file must refer to [LICENSE](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/LICENSE) +2. Every example / source file must include correct copyright notice +3. For indentation we are using spaces and not tabs +4. Line-endings must be Unix and not DOS style + +Most common issues can be automatically resolved by running `./pr_check.sh --fix` + +Please familirize yourself with the [Apache License 2.0](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/LICENSE) before contributing. + +## Step-by-Step + +1. Make sure you have `git` and `cmake` installed on your system. On Windows we recommend using [Git Extensions](https://github.com/gitextensions/gitextensions/releases) for git bash. +2. Run `git clone https://github.com/IntelRealSense/realsense-ros.git` and `cd realsense-ros` +3. To align with latest status of the ros2-development branch, run: +``` +git fetch origin +git checkout ros2-development +git reset --hard origin/ros2-development +``` +4. `git checkout -b name_of_your_contribution` to create a dedicated branch +5. Make your changes to the local repository +6. Make sure your local git user is updated, or run `git config --global user.email "email@example.com"` and `git config --global user.user "user"` to set it up. This is the user & email that will appear in GitHub history. +7. `git add -p` to select the changes you wish to add +8. `git commit -m "Description of the change"` +9. Make sure you have a GitHub user and [fork realsense-ros](https://github.com/IntelRealSense/realsense-ros#fork-destination-box) +10. `git remote add fork https://github.com/username/realsense-ros.git` with your GitHub `username` +11. `git fetch fork` +12. `git push fork` to push `name_of_your_contribution` branch to your fork +13. Go to your fork on GitHub at `https://github.com/username/realsense-ros` +14. Click the `New pull request` button +15. For `base` combo-box select `ros2-development`, since you want to submit a PR to that branch +16. For `compare` combo-box select `name_of_your_contribution` with your commit +17. Review your changes and click `Create pull request` +18. Wait for all automated checks to pass +19. The PR will be approved / rejected after review from the team and the community + +To continue to new change, goto step 3. +To return to your PR (in order to make more changes): +1. `git stash` +2. `git checkout name_of_your_contribution` +3. Repeat items 5-8 from the previous list +4. `git push fork` +The pull request will be automatically updated + diff --git a/LICENSE b/LICENSE index f3a4cc68a3..5444cbd69d 100644 --- a/LICENSE +++ b/LICENSE @@ -187,7 +187,7 @@ same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright 2022 Intel Corporation + 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. diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index c6f7f45bb2..59d56a114c 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. cmake_minimum_required(VERSION 3.5) project(realsense2_camera) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4a88f9496e..6d9b93e297 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 8dbb919b8d..cf1de15d76 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera/include/dynamic_params.h b/realsense2_camera/include/dynamic_params.h index 208415df7c..ca6fa5271b 100644 --- a/realsense2_camera/include/dynamic_params.h +++ b/realsense2_camera/include/dynamic_params.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once #include diff --git a/realsense2_camera/include/image_publisher.h b/realsense2_camera/include/image_publisher.h index 844fdb9c5c..6bc0bab8e6 100644 --- a/realsense2_camera/include/image_publisher.h +++ b/realsense2_camera/include/image_publisher.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera/include/named_filter.h b/realsense2_camera/include/named_filter.h index 5a481a5f58..88ce2a85a3 100644 --- a/realsense2_camera/include/named_filter.h +++ b/realsense2_camera/include/named_filter.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera/include/profile_manager.h b/realsense2_camera/include/profile_manager.h index 2e8383fa1f..2e55f985b7 100644 --- a/realsense2_camera/include/profile_manager.h +++ b/realsense2_camera/include/profile_manager.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index e133814860..da807ac7a2 100755 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera/include/ros_param_backend.h b/realsense2_camera/include/ros_param_backend.h index aba3423cb8..735a29d9d4 100644 --- a/realsense2_camera/include/ros_param_backend.h +++ b/realsense2_camera/include/ros_param_backend.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h index 673ec1c57f..004f42b40c 100644 --- a/realsense2_camera/include/ros_sensor.h +++ b/realsense2_camera/include/ros_sensor.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index 8b8bf69f5a..55b3dafa13 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera/include/sensor_params.h b/realsense2_camera/include/sensor_params.h index ea8ba5fb37..1e1cbb974b 100644 --- a/realsense2_camera/include/sensor_params.h +++ b/realsense2_camera/include/sensor_params.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once #include diff --git a/realsense2_camera/launch/rs_intra_process_demo_launch.py b/realsense2_camera/launch/rs_intra_process_demo_launch.py index 12d64d6b87..6c240b5e2a 100644 --- a/realsense2_camera/launch/rs_intra_process_demo_launch.py +++ b/realsense2_camera/launch/rs_intra_process_demo_launch.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. ''' Launch realsense2_camera node & a frame latency printer node, diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index fd0310f9a6..c05644d01f 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. """Launch realsense2_camera node.""" import os diff --git a/realsense2_camera/launch/rs_multi_camera_launch.py b/realsense2_camera/launch/rs_multi_camera_launch.py index ece2f36a23..bce56ee1e7 100644 --- a/realsense2_camera/launch/rs_multi_camera_launch.py +++ b/realsense2_camera/launch/rs_multi_camera_launch.py @@ -1,6 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. - +# 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 # # ----------- # diff --git a/realsense2_camera/scripts/echo_metadada.py b/realsense2_camera/scripts/echo_metadada.py index a243b851c0..534422a85e 100644 --- a/realsense2_camera/scripts/echo_metadada.py +++ b/realsense2_camera/scripts/echo_metadada.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. #!/usr/bin/env python import os diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index c61dd467b0..87f796041e 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. import sys import time diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index f8f9c11065..4e27ba9e9c 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. import os import sys diff --git a/realsense2_camera/scripts/set_cams_transforms.py b/realsense2_camera/scripts/set_cams_transforms.py index 3d7ca73057..aa0186cac7 100644 --- a/realsense2_camera/scripts/set_cams_transforms.py +++ b/realsense2_camera/scripts/set_cams_transforms.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. import rospy import sys diff --git a/realsense2_camera/scripts/show_center_depth.py b/realsense2_camera/scripts/show_center_depth.py index 2a03ea490a..fb957664f4 100644 --- a/realsense2_camera/scripts/show_center_depth.py +++ b/realsense2_camera/scripts/show_center_depth.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. import rclpy from rclpy.node import Node diff --git a/realsense2_camera/scripts/topic_hz.py b/realsense2_camera/scripts/topic_hz.py index 1820744519..3864015891 100644 --- a/realsense2_camera/scripts/topic_hz.py +++ b/realsense2_camera/scripts/topic_hz.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. import rclpy from rclpy.node import Node diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 671e794dfe..c543663c42 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include "../include/base_realsense_node.h" #include "assert.h" diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index 7622f54aad..3c9cd304d6 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include diff --git a/realsense2_camera/src/image_publisher.cpp b/realsense2_camera/src/image_publisher.cpp index e6dc8408b5..36525fa03a 100644 --- a/realsense2_camera/src/image_publisher.cpp +++ b/realsense2_camera/src/image_publisher.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include diff --git a/realsense2_camera/src/named_filter.cpp b/realsense2_camera/src/named_filter.cpp index 59efaa9d65..4e08042632 100644 --- a/realsense2_camera/src/named_filter.cpp +++ b/realsense2_camera/src/named_filter.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include #include diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index fd460d9888..053704c067 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include "../include/base_realsense_node.h" #include diff --git a/realsense2_camera/src/profile_manager.cpp b/realsense2_camera/src/profile_manager.cpp index a6f11489d6..d86c6358aa 100644 --- a/realsense2_camera/src/profile_manager.cpp +++ b/realsense2_camera/src/profile_manager.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include #include diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index b5f68ef2b2..92984b8590 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved +// 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. #include "../include/realsense_node_factory.h" #include "../include/base_realsense_node.h" diff --git a/realsense2_camera/src/ros_param_backend_dashing.cpp b/realsense2_camera/src/ros_param_backend_dashing.cpp index 4afdb53b23..8ecf8e7e8f 100644 --- a/realsense2_camera/src/ros_param_backend_dashing.cpp +++ b/realsense2_camera/src/ros_param_backend_dashing.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include "ros_param_backend.h" diff --git a/realsense2_camera/src/ros_param_backend_foxy.cpp b/realsense2_camera/src/ros_param_backend_foxy.cpp index daceb7e171..e3998e9808 100644 --- a/realsense2_camera/src/ros_param_backend_foxy.cpp +++ b/realsense2_camera/src/ros_param_backend_foxy.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include "ros_param_backend.h" diff --git a/realsense2_camera/src/ros_param_backend_rolling.cpp b/realsense2_camera/src/ros_param_backend_rolling.cpp index daceb7e171..e3998e9808 100644 --- a/realsense2_camera/src/ros_param_backend_rolling.cpp +++ b/realsense2_camera/src/ros_param_backend_rolling.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include "ros_param_backend.h" diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 0927c37520..541b07bc04 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include diff --git a/realsense2_camera/src/ros_utils.cpp b/realsense2_camera/src/ros_utils.cpp index bc722d2383..e5fa1c3817 100644 --- a/realsense2_camera/src/ros_utils.cpp +++ b/realsense2_camera/src/ros_utils.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include #include diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 6d7245ee70..ebbbaf3285 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include "../include/base_realsense_node.h" #include diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index b6e8a7fb3e..ff0cf151e9 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include #include diff --git a/realsense2_camera/tools/frame_latency/frame_latency.cpp b/realsense2_camera/tools/frame_latency/frame_latency.cpp index cac7946f1c..022e733e49 100644 --- a/realsense2_camera/tools/frame_latency/frame_latency.cpp +++ b/realsense2_camera/tools/frame_latency/frame_latency.cpp @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #include #include diff --git a/realsense2_camera/tools/frame_latency/frame_latency.h b/realsense2_camera/tools/frame_latency/frame_latency.h index 5250e10df3..653c648578 100644 --- a/realsense2_camera/tools/frame_latency/frame_latency.h +++ b/realsense2_camera/tools/frame_latency/frame_latency.h @@ -1,5 +1,16 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2022 Intel Corporation. All Rights Reserved. +// 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. #pragma once diff --git a/realsense2_camera_msgs/CMakeLists.txt b/realsense2_camera_msgs/CMakeLists.txt index 505e9f61ae..a1203e88c7 100644 --- a/realsense2_camera_msgs/CMakeLists.txt +++ b/realsense2_camera_msgs/CMakeLists.txt @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. cmake_minimum_required(VERSION 3.5) diff --git a/realsense2_description/CMakeLists.txt b/realsense2_description/CMakeLists.txt index 1fd88370c8..30c04d9fa8 100644 --- a/realsense2_description/CMakeLists.txt +++ b/realsense2_description/CMakeLists.txt @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. cmake_minimum_required(VERSION 3.5) project(realsense2_description) diff --git a/realsense2_description/launch/launch_utils.py b/realsense2_description/launch/launch_utils.py index f88f6a09c6..c25d1f7086 100644 --- a/realsense2_description/launch/launch_utils.py +++ b/realsense2_description/launch/launch_utils.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. import os import xacro diff --git a/realsense2_description/launch/view_model.launch.py b/realsense2_description/launch/view_model.launch.py index 0b0310023e..43caafc2b0 100644 --- a/realsense2_description/launch/view_model.launch.py +++ b/realsense2_description/launch/view_model.launch.py @@ -1,5 +1,16 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright (c) 2022 Intel Corporation +# 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. # /* Author: Doron Hirshberg */ import os diff --git a/realsense2_description/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro index 9b2f067840..282c764cb4 100644 --- a/realsense2_description/urdf/_d415.urdf.xacro +++ b/realsense2_description/urdf/_d415.urdf.xacro @@ -1,8 +1,21 @@ + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index f23ddc18f0..a8e11faffe 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -1,9 +1,22 @@ + diff --git a/realsense2_description/urdf/_d435i.urdf.xacro b/realsense2_description/urdf/_d435i.urdf.xacro index 9f18badbbe..9acd8d871c 100644 --- a/realsense2_description/urdf/_d435i.urdf.xacro +++ b/realsense2_description/urdf/_d435i.urdf.xacro @@ -1,8 +1,22 @@ + + diff --git a/realsense2_description/urdf/_d435i_imu_modules.urdf.xacro b/realsense2_description/urdf/_d435i_imu_modules.urdf.xacro index 3bab0fa9ca..cac7f4b367 100644 --- a/realsense2_description/urdf/_d435i_imu_modules.urdf.xacro +++ b/realsense2_description/urdf/_d435i_imu_modules.urdf.xacro @@ -1,9 +1,22 @@ + diff --git a/realsense2_description/urdf/_d455.urdf.xacro b/realsense2_description/urdf/_d455.urdf.xacro index e36a5d4935..509d9720d2 100644 --- a/realsense2_description/urdf/_d455.urdf.xacro +++ b/realsense2_description/urdf/_d455.urdf.xacro @@ -1,9 +1,22 @@ + diff --git a/realsense2_description/urdf/_l515.urdf.xacro b/realsense2_description/urdf/_l515.urdf.xacro index dff77fe491..40d7f58619 100644 --- a/realsense2_description/urdf/_l515.urdf.xacro +++ b/realsense2_description/urdf/_l515.urdf.xacro @@ -1,9 +1,22 @@ + diff --git a/realsense2_description/urdf/_materials.urdf.xacro b/realsense2_description/urdf/_materials.urdf.xacro index 58205b874b..9a62789d8a 100644 --- a/realsense2_description/urdf/_materials.urdf.xacro +++ b/realsense2_description/urdf/_materials.urdf.xacro @@ -1,9 +1,22 @@ + diff --git a/realsense2_description/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro index a64ab6e3ee..f55028507a 100644 --- a/realsense2_description/urdf/_r410.urdf.xacro +++ b/realsense2_description/urdf/_r410.urdf.xacro @@ -1,9 +1,22 @@ + diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index fd991b732c..0b942abe9a 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -1,9 +1,22 @@ + diff --git a/realsense2_description/urdf/_usb_plug.urdf.xacro b/realsense2_description/urdf/_usb_plug.urdf.xacro index cc67573c06..ba25abcab2 100644 --- a/realsense2_description/urdf/_usb_plug.urdf.xacro +++ b/realsense2_description/urdf/_usb_plug.urdf.xacro @@ -1,8 +1,21 @@ + diff --git a/scripts/pr_check.sh b/scripts/pr_check.sh index 75dae5843c..837be9f058 100755 --- a/scripts/pr_check.sh +++ b/scripts/pr_check.sh @@ -1,7 +1,18 @@ #!/bin/bash -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +# 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. # This script makes sure all files with the following extensions - .h, .hpp, .cpp, .js, .py, .bat, .sh, .txt - # include the Apache license reference and Intel copyright, as it shown in this file header. @@ -16,12 +27,13 @@ sudo apt-get install dos2unix ok=0 fixed=0 +license_file=$PWD/../LICENSE function check_folder { for filename in $(find $1 -type f \( -iname \*.cpp -o -iname \*.h -o -iname \*.hpp -o -iname \*.js -o -iname \*.bat -o -iname \*.sh -o -iname \*.txt -o -iname \*.py \)); do # Skip files of 3rd-party libraries which already have their own licenses and copyrights - if [[ "$filename" == *"importRosbag"* ]]; then + if [[ "$filename" == *"importRosbag"* || "$filename" == *"pr_check.sh"* ]]; then continue; fi @@ -31,43 +43,30 @@ function check_folder { if [[ ! $filename == *"usbhost"* ]]; then # Only check files that are not .gitignore-d if [[ $(git check-ignore $filename | wc -l) -eq 0 ]]; then - if [[ $(grep -oP "(?<=\(c\) )(.*)(?= Intel)" $filename | wc -l) -eq 0 ]]; then - echo "[ERROR] $filename is missing the copyright notice" - ok=$((ok+1)) - - if [[ $2 == *"fix"* ]]; then - if [[ $(date +%Y) == "2022" ]]; then - if [[ $filename == *".h"* || $filename == *".hpp"* || $filename == *".cpp"* || $filename == *".js"* ]]; then - echo "Trying to auto-resolve..."; - ex -sc '1i|// Copyright(c) 2022 Intel Corporation. All Rights Reserved.' -cx $filename - fixed=$((fixed+1)) - fi - if [[ $filename == *".txt"* || $filename == *".py"* ]]; then - echo "Trying to auto-resolve..."; - ex -sc '1i|# Copyright(c) 2022 Intel Corporation. All Rights Reserved.' -cx $filename - fixed=$((fixed+1)) - fi - else - echo Please update pr_check to auto-resolve missing copyright - fi - fi - fi - - if [[ $(grep -oP "Apache 2.0" $filename | wc -l) -eq 0 ]]; then - echo "[ERROR] $filename is missing license notice" + if [[ $(grep -oP "Copyright 2023 Intel Corporation. All Rights Reserved" $filename | wc -l) -eq 0 || + $(grep -oP "Licensed under the Apache License, Version 2.0" $filename | wc -l) -eq 0 + ]]; then + echo "[ERROR] $filename is missing the copyright/license notice" ok=$((ok+1)) if [[ $2 == *"fix"* ]]; then + # take last 13 linse from LICENSE file, and put them at beginning of $filename if [[ $filename == *".h"* || $filename == *".hpp"* || $filename == *".cpp"* || $filename == *".js"* ]]; then - echo "Trying to auto-resolve..."; - ex -sc '1i|// License: Apache 2.0. See LICENSE file in root directory.' -cx $filename - fixed=$((fixed+1)) + license_str="$(tail -13 ${license_file} | sed -e 's/^ / /' | sed -e 's/^/\/\//')" fi if [[ $filename == *".txt"* || $filename == *".py"* ]]; then - echo "Trying to auto-resolve..."; - ex -sc '1i|# License: Apache 2.0. See LICENSE file in root directory.' -cx $filename - fixed=$((fixed+1)) + license_str="$(tail -13 ${license_file} | sed -e 's/^ / /' | sed -e 's/^/#/')" fi + echo "Trying to auto-resolve..."; + ed -s $filename << END +0i +${license_str} + +. +w +q +END + fixed=$((fixed+1)) fi fi From 4404564f1fd48816c091c5bce06aef2819e36ec3 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 1 Feb 2023 23:33:56 +0200 Subject: [PATCH 39/85] add static_analysis.yaml to project and fix some code to pass cppcheck run --- .github/workflows/static_analysis.yaml | 60 +++++++++++++++++++ .../include/base_realsense_node.h | 4 +- realsense2_camera/include/dynamic_params.h | 2 + realsense2_camera/src/base_realsense_node.cpp | 19 ++++-- realsense2_camera/src/dynamic_params.cpp | 3 +- 5 files changed, 79 insertions(+), 9 deletions(-) create mode 100644 .github/workflows/static_analysis.yaml diff --git a/.github/workflows/static_analysis.yaml b/.github/workflows/static_analysis.yaml new file mode 100644 index 0000000000..7672fd6956 --- /dev/null +++ b/.github/workflows/static_analysis.yaml @@ -0,0 +1,60 @@ +name: static_analysis + +on: + push: + branches: ['**'] + pull_request: + branches: ['**'] + +jobs: + cppcheck: + name: cppcheck + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v3 + + - name: Install + shell: bash + run: | + sudo apt-get update; + sudo apt-get install -qq cppcheck; + + - name: Cppcheck Run + shell: bash + #Selected run options: + # ./xxx : Folders to scan + # --quiet : Don't show current checked configuration in log + # --std=c++11 : Use C++11 standard (default but worth mentioning) + # --xml : Output in XML format + # -j4 : Run parallel jobs for a faster scan. current HW is 2 core (https://docs.github.com/en/actions/using-github-hosted-runners/about-github-hosted-runners) using 4 to future proof + # --enable : Additional check to run. options are [all, warning, style, performance, protability, information, unusedFunction, missingInclude] + # -I : Include directories + # -i : Ignore directories. Ignore third-party libs that we don't want to check + # --suppress : Don't issue errors about files matching the expression (when -i for folders is not enough) + # --force : Check all configurations, takes a very long time (~2 hours) and did not find additional errors. Removed. + # --max-configs=6 : Using less configuration permutations (default is 12) to reduce run time. Detects less errors. Removed. + # -Dxxx : preprocessor configuration to use. Relevant flags taken from build on Ubuntu. + run: > + cppcheck ./realsense2_camera/src ./realsense2_camera/include ./realsense2_camera/tools + --quiet --std=c++11 --xml -j4 --enable=warning + -I./realsense2_camera/include -I./realsense2_camera/tools + &> cppcheck_run.log + + - name: Cppcheck Result + shell: bash + run: | + ERROR_COUNT=$(grep cppcheck_run.log -e "severity=\"error\"" -c) || ERROR_COUNT=0 + EXPECTED_ERROR_COUNT=0 + if [ $ERROR_COUNT -eq $EXPECTED_ERROR_COUNT ] + then + echo "cppcheck_run succeeded, found" $ERROR_COUNT "errors, as expected" + exit 0 + elif [ $ERROR_COUNT -lt $EXPECTED_ERROR_COUNT ] + then + echo "cppcheck_run ---> SUCCEEDED <--- but found" $ERROR_COUNT "errors when expecting" $EXPECTED_ERROR_COUNT + echo "Please update EXPECTED_ERROR_COUNT var in .github/workflows/static_analysis.yaml to the new lower value" + else + echo "cppcheck_run ---> FAILED <--- with" $ERROR_COUNT "errors; expecting" $EXPECTED_ERROR_COUNT + fi + cat cppcheck_run.log + exit 1 diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4a88f9496e..26a03532b0 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -77,7 +77,6 @@ namespace realsense2_camera class SyncedImuPublisher { public: - SyncedImuPublisher() {_is_enabled=false;}; SyncedImuPublisher(rclcpp::Publisher::SharedPtr imu_publisher, std::size_t waiting_list_size=1000); ~SyncedImuPublisher(); @@ -146,7 +145,7 @@ namespace realsense2_camera std::list _parameters_names; void publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex); - virtual void calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile); + void calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile); void getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res); tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const; @@ -247,6 +246,7 @@ namespace realsense2_camera std::mutex _publish_tf_mutex; std::mutex _update_sensor_mutex; std::mutex _profile_changes_mutex; + std::mutex _publish_dynamic_tf_mutex; std::shared_ptr _static_tf_broadcaster; std::shared_ptr _dynamic_tf_broadcaster; diff --git a/realsense2_camera/include/dynamic_params.h b/realsense2_camera/include/dynamic_params.h index 208415df7c..0950d2a644 100644 --- a/realsense2_camera/include/dynamic_params.h +++ b/realsense2_camera/include/dynamic_params.h @@ -50,5 +50,7 @@ namespace realsense2_camera std::shared_ptr _update_functions_t; std::deque > _update_functions_v; std::list self_set_parameters; + std::mutex _mu; + }; } diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 671e794dfe..a97ef85b8d 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -22,7 +22,7 @@ using namespace realsense2_camera; SyncedImuPublisher::SyncedImuPublisher(rclcpp::Publisher::SharedPtr imu_publisher, std::size_t waiting_list_size): _publisher(imu_publisher), _pause_mode(false), - _waiting_list_size(waiting_list_size) + _waiting_list_size(waiting_list_size), _is_enabled(false) {} SyncedImuPublisher::~SyncedImuPublisher() @@ -87,10 +87,21 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _parameters(parameters), _dev(dev), _json_file_path(""), + _depth_scale_meters(0), + _clipping_distance(0), + _linear_accel_cov(0), + _angular_velocity_cov(0), + _hold_back_imu_for_frames(false), + _publish_tf(false), _tf_publish_rate(TF_PUBLISH_RATE), + _diagnostics_period(0), _use_intra_process(use_intra_process), _is_initialized_time_base(false), + _camera_time_base(0), _sync_frames(SYNC_FRAMES), + _pointcloud(false), + _publish_odom_tf(false), + _imu_sync_method(imu_sync_method::NONE), _is_profile_changed(false), _is_align_depth_changed(false) { @@ -918,7 +929,7 @@ void BaseRealSenseNode::SetBaseStream() } std::vector::const_iterator base_stream(base_stream_priority.begin()); - while( (available_profiles.find(*base_stream) == available_profiles.end()) && (base_stream != base_stream_priority.end())) + while((base_stream != base_stream_priority.end()) && (available_profiles.find(*base_stream) == available_profiles.end())) { base_stream++; } @@ -973,9 +984,7 @@ void BaseRealSenseNode::startDynamicTf() void BaseRealSenseNode::publishDynamicTransforms() { // Publish transforms for the cameras - - std::mutex mu; - std::unique_lock lock(mu); + std::unique_lock lock(_publish_dynamic_tf_mutex); while (rclcpp::ok() && _is_running && _tf_publish_rate > 0) { _cv_tf.wait_for(lock, std::chrono::milliseconds((int)(1000.0/_tf_publish_rate)), [&]{return (!(_is_running && _tf_publish_rate > 0));}); diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index 7622f54aad..3873ccea5c 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -59,8 +59,7 @@ namespace realsense2_camera { int time_interval(1000); std::function func = [this, time_interval](){ - std::mutex mu; - std::unique_lock lock(mu); + std::unique_lock lock(_mu); while(_is_running) { _update_functions_cv.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return !_is_running || !_update_functions_v.empty();}); while (!_update_functions_v.empty()) From 9b60cfb6021d7faf9477c72420934d030073a1a7 Mon Sep 17 00:00:00 2001 From: NirAz Date: Thu, 9 Feb 2023 11:03:25 +0200 Subject: [PATCH 40/85] restore extrinsic topic qos to latched --- .../include/base_realsense_node.h | 1 - realsense2_camera/src/base_realsense_node.cpp | 18 ------------------ realsense2_camera/src/rs_node_setup.cpp | 10 ++++++---- 3 files changed, 6 insertions(+), 23 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 7a657e135b..de90f2e7fd 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -275,7 +275,6 @@ namespace realsense2_camera std::map::SharedPtr> _metadata_publishers; std::map::SharedPtr> _imu_info_publisher; std::map::SharedPtr> _extrinsics_publishers; - std::map _extrinsics_msgs; std::map _image; std::map _encoding; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index cbbbca5c95..feefe9fa04 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -864,7 +864,6 @@ void BaseRealSenseNode::publishExtrinsicsTopic(const stream_index_pair& sip, con Extrinsics msg = rsExtrinsicsToMsg(ex); if (_extrinsics_publishers.find(sip) != _extrinsics_publishers.end()) { - _extrinsics_msgs[sip] = msg; // We keep the message for periodically publish later if needed _extrinsics_publishers[sip]->publish(msg); } } @@ -1012,23 +1011,6 @@ void BaseRealSenseNode::publishDynamicTransforms() { ROS_ERROR_STREAM("Error publishing dynamic transforms: " << e.what()); } - - // If static_tf was not created we need to publish the extrinsics periodically since it is not publishes as a latched topic. - if ( !_static_tf_broadcaster ) - { - try - { - for (const auto &extrinsics_publisher : _extrinsics_publishers) - { - const auto &ext_msg = _extrinsics_msgs[extrinsics_publisher.first]; - extrinsics_publisher.second->publish( ext_msg ); - } - } - catch (const std::exception &e) - { - ROS_ERROR_STREAM("Error publishing extrinsics : " << e.what()); - } - } } } } diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index ebbbaf3285..91335d0ace 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -195,7 +195,6 @@ void BaseRealSenseNode::stopPublishers(const std::vector& profil } _metadata_publishers.erase(sip); _extrinsics_publishers.erase(sip); - _extrinsics_msgs.erase(sip); } } @@ -286,12 +285,15 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile)) { - // When intra process is on we cannot use latched qos, we will need to send this message periodically with volatile durability - rmw_qos_profile_t extrinsics_qos = _use_intra_process ? rmw_qos_profile_default : rmw_qos_profile_latched; + + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + rmw_qos_profile_t extrinsics_qos = rmw_qos_profile_latched; + std::string topic_extrinsics("extrinsics/" + create_graph_resource_name(ros_stream_to_string(_base_profile.stream_type()) + "_to_" + stream_name)); _extrinsics_publishers[sip] = _node.create_publisher(topic_extrinsics, - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(extrinsics_qos), extrinsics_qos)); + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(extrinsics_qos), extrinsics_qos), std::move(options)); } } } From dd2f69058284e0052f18ddc393c54738cbcc644f Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Thu, 9 Feb 2023 14:04:54 +0200 Subject: [PATCH 41/85] add comments --- realsense2_camera/src/base_realsense_node.cpp | 1 + realsense2_camera/src/rs_node_setup.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index feefe9fa04..2456f477dd 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -121,6 +121,7 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, ROS_INFO("Intra-Process communication enabled"); } + // intra-process do not support latched QoS, so we need to disable intra-process for this topic rclcpp::PublisherOptionsWithAllocator> options; options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; #ifndef DASHING diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 91335d0ace..762e760126 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -286,7 +286,7 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile)) { - + // intra-process do not support latched QoS, so we need to disable intra-process for this topic rclcpp::PublisherOptionsWithAllocator> options; options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; rmw_qos_profile_t extrinsics_qos = rmw_qos_profile_latched; From 7d77cb015cb7a3b3a6fd895467c6fc7eeec6387f Mon Sep 17 00:00:00 2001 From: Stephan Wirth <1481786+stwirth@users.noreply.github.com> Date: Mon, 13 Mar 2023 11:54:39 +0100 Subject: [PATCH 42/85] Fix #2649: Expose USB port in DeviceInfo service --- realsense2_camera/src/rs_node_setup.cpp | 1 + realsense2_camera_msgs/srv/DeviceInfo.srv | 1 + 2 files changed, 2 insertions(+) diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 762e760126..3648fc3c8a 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -390,4 +390,5 @@ void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceI } res->sensors = sensors_names.str().substr(0, sensors_names.str().size()-1); + res->physical_port = _dev.supports(RS2_CAMERA_INFO_PHYSICAL_PORT) ? _dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT) : ""; } diff --git a/realsense2_camera_msgs/srv/DeviceInfo.srv b/realsense2_camera_msgs/srv/DeviceInfo.srv index d41600c639..3c9b2ef4e7 100644 --- a/realsense2_camera_msgs/srv/DeviceInfo.srv +++ b/realsense2_camera_msgs/srv/DeviceInfo.srv @@ -5,3 +5,4 @@ string firmware_version string usb_type_descriptor string firmware_update_id string sensors +string physical_port From b54cf11fcac44fe305563e098854eaa37cfdc916 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 14 Mar 2023 12:43:55 +0200 Subject: [PATCH 43/85] fix timestamp calculation metadata header to be aligned with metadata json timestamp --- .../include/base_realsense_node.h | 1 + realsense2_camera/src/base_realsense_node.cpp | 24 ++++++++++++++++--- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index de90f2e7fd..2047994a45 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -192,6 +192,7 @@ namespace realsense2_camera void enable_devices(); void setupFilters(); bool setBaseTime(double frame_time, rs2_timestamp_domain time_domain); + uint64_t millisecondsToNanoseconds(double timestamp_ms); rclcpp::Time frameSystemTimeSec(rs2::frame frame); cv::Mat& fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image); void clip_depth(rs2::depth_frame depth_frame, float clipping_dist); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 2456f477dd..43cd26eb14 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -708,17 +708,35 @@ bool BaseRealSenseNode::setBaseTime(double frame_time, rs2_timestamp_domain time return false; } +uint64_t BaseRealSenseNode::millisecondsToNanoseconds(double timestamp_ms) +{ + // modf breaks input into an integral and fractional part + double int_part_ms, fract_part_ms; + fract_part_ms = modf(timestamp_ms, &int_part_ms); + + //convert to ns + uint64_t int_part_ns = static_cast(int_part_ms) * 1000000; + uint64_t fract_part_ns = static_cast(fract_part_ms * 10000000); + + // Convert fract_parts_ns into 6 digits and round it + // to be aligned with librealsense get_timestamp API + fract_part_ns = (fract_part_ns % 10 > 4) ? (fract_part_ns/10 + 1) : (fract_part_ns/10); + + return int_part_ns + fract_part_ns; +} + rclcpp::Time BaseRealSenseNode::frameSystemTimeSec(rs2::frame frame) { + double timestamp_ms = frame.get_timestamp(); if (frame.get_frame_timestamp_domain() == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) { - double elapsed_camera_ns = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) * 1e6; + double elapsed_camera_ns = millisecondsToNanoseconds(timestamp_ms - _camera_time_base); /* Fixing deprecated-declarations compilation warning. Duration(rcl_duration_value_t) is deprecated in favor of static Duration::from_nanoseconds(rcl_duration_value_t) - starting from GALAXY. + starting from GALACTIC. */ #if defined(FOXY) || defined(ELOQUENT) || defined(DASHING) auto duration = rclcpp::Duration(elapsed_camera_ns); @@ -730,7 +748,7 @@ rclcpp::Time BaseRealSenseNode::frameSystemTimeSec(rs2::frame frame) } else { - return rclcpp::Time(frame.get_timestamp() * 1e6); + return rclcpp::Time(millisecondsToNanoseconds(timestamp_ms)); } } From baadd0f6a7e6ae4549b04e3de8ec160b6db7e63d Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 19 Mar 2023 15:39:22 +0200 Subject: [PATCH 44/85] fix code review comments --- realsense2_camera/src/base_realsense_node.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 43cd26eb14..4c18a320b3 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -714,13 +714,13 @@ uint64_t BaseRealSenseNode::millisecondsToNanoseconds(double timestamp_ms) double int_part_ms, fract_part_ms; fract_part_ms = modf(timestamp_ms, &int_part_ms); - //convert to ns - uint64_t int_part_ns = static_cast(int_part_ms) * 1000000; - uint64_t fract_part_ns = static_cast(fract_part_ms * 10000000); - - // Convert fract_parts_ns into 6 digits and round it + //convert both parts to ns + static constexpr uint64_t milli_to_nano = 1000000; + uint64_t int_part_ns = static_cast(int_part_ms) * milli_to_nano; + // for fract_part_ns, multiplie ns * 10, then divide by 10 while rounding // to be aligned with librealsense get_timestamp API - fract_part_ns = (fract_part_ns % 10 > 4) ? (fract_part_ns/10 + 1) : (fract_part_ns/10); + uint64_t fract_part_ns = static_cast(fract_part_ms * milli_to_nano * 10); + fract_part_ns = (fract_part_ns + 5)/10; return int_part_ns + fract_part_ns; } From 453868220d207b6badaf8ced19c311bb2f7aedc5 Mon Sep 17 00:00:00 2001 From: Stephan Wirth <1481786+stwirth@users.noreply.github.com> Date: Mon, 13 Mar 2023 11:54:39 +0100 Subject: [PATCH 45/85] Fix #2649: Expose USB port in DeviceInfo service --- realsense2_camera/src/rs_node_setup.cpp | 1 + realsense2_camera_msgs/srv/DeviceInfo.srv | 1 + 2 files changed, 2 insertions(+) diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 762e760126..3648fc3c8a 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -390,4 +390,5 @@ void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceI } res->sensors = sensors_names.str().substr(0, sensors_names.str().size()-1); + res->physical_port = _dev.supports(RS2_CAMERA_INFO_PHYSICAL_PORT) ? _dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT) : ""; } diff --git a/realsense2_camera_msgs/srv/DeviceInfo.srv b/realsense2_camera_msgs/srv/DeviceInfo.srv index d41600c639..3c9b2ef4e7 100644 --- a/realsense2_camera_msgs/srv/DeviceInfo.srv +++ b/realsense2_camera_msgs/srv/DeviceInfo.srv @@ -5,3 +5,4 @@ string firmware_version string usb_type_descriptor string firmware_update_id string sensors +string physical_port From b57eaeea668e276a75c6eeb5cccbb2d9807fecc0 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 22 Mar 2023 16:44:02 +0200 Subject: [PATCH 46/85] update rs_launch.py to support enable_auto_exposure and manual exposure --- realsense2_camera/launch/rs_launch.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index c05644d01f..fdb6c2789c 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -33,6 +33,7 @@ {'name': 'depth_module.profile', 'default': '0,0,0', 'description': 'depth module profile'}, {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, {'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'}, + {'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'}, {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, @@ -65,7 +66,9 @@ {'name': 'depth_module.gain.1', 'default': '16', 'description': 'Depth module first gain value. Used for hdr_merge filter'}, {'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': 'depth_module.exposure', 'default': '8500', 'description': 'Depth module manual exposure value'}, {'name': 'depth_module.hdr_enabled', 'default': 'false', 'description': 'Depth module hdr enablement flag. Used for hdr_merge filter'}, + {'name': 'depth_module.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for depth image'}, {'name': 'hdr_merge.enable', 'default': 'false', 'description': 'hdr_merge filter enablement flag'}, {'name': 'wait_for_device_timeout', 'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'}, {'name': 'reconnect_timeout', 'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'}, From 1cdd122dd9d164d06b85a69ad576dc7dfc22e95b Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Sun, 26 Mar 2023 01:23:13 +0300 Subject: [PATCH 47/85] reorder README.md --- README.md | 642 +++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 470 insertions(+), 172 deletions(-) diff --git a/README.md b/README.md index 6659fdb695..55d2b7498a 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,47 @@ -# ROS2 Wrapper for Intel® RealSense™ Devices -These are packages for using Intel RealSense cameras (D400, L500 and SR300 cameras) with ROS2. - -This version supports ROS2 Dashing, Eloquent, Foxy, Galactic and Rolling. - -LibRealSense supported version: v2.51.1 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) - - -## For LibRS ROS1 Wrapper please refer to [ROS1-legacy branch](https://github.com/IntelRealSense/realsense-ros/tree/ros1-legacy) - -## Please notice: if you are moving from RealSense [ROS2-legacy branch](https://github.com/IntelRealSense/realsense-ros/tree/ros2-legacy) to ROS2-development: -- **Changed Parameters**: +

+ Intel® RealSense™ +

+ +

+ ROS2 packages for using Intel RealSense D400 cameras.
+ Supported ROS2 Distros: Dashing, Eloquent, Foxy, Humble and Rolling.
+ Latest release notes +

+ +
+ +## Table of contents + * [ROS1 and ROS2 legacy](#legacy) + * [Installation](#installation) + * [Usage](#usage) + * [Starting the camera node](#start-camera-node) + * [Parameters](#parameters) + * [Topics](#topics) + * [Metadata Topic](#metadata) + * [Post-Processing Filters](#filters) + * [Available Services](#services) + * [Efficient intra-process communication](#intra-process) + * [Contributing](#contributing) + * [License](#license) + +

+ Legacy +

+ +
+ + Intel RealSense ROS1 Wrapper + + Intel Realsense ROS1 Wrapper is not supported anymore, since our developers team are focusing on ROS2 distro.
+ For ROS1 wrapper, go to ros1-legacy branch +
+ +
+ + Moving from ros2-legacy to ros2-development + + +* Changed Parameters: - **"stereo_module"**, **"l500_depth_sensor"** are replaced by **"depth_module"** - For video streams: **\.profile** replaces **\_width**, **\_height**, **\_fps** - **ROS2-legacy (Old)**: @@ -26,45 +58,75 @@ LibRealSense supported version: v2.51.1 (see [realsense2_camera release notes](h - **unite_imu_method** parameter is now changeable in runtime. - **enable_sync** parameter is now changeable in runtime. - - -## Installation Instructions - - ### Step 1: Install the ROS2 distribution - - #### Ubuntu 22.04: - - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) - - #### Ubuntu 20.04: - - [ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) - - [ROS2 Galactic](https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html) - - #### Ubuntu 18.04 : - - [ROS2 Dashing](https://docs.ros.org/en/dashing/Installation/Ubuntu-Install-Debians.html) - - [ROS2 Eloquent](https://docs.ros.org/en/eloquent/Installation/Linux-Install-Debians.html) - - -### Step 2: Install the latest Intel® RealSense™ SDK 2.0 - -- #### Option 1: Install librealsense2 debian package (Not supported in Ubuntu 22.04 yet) - - Jetson users - use the [Jetson Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_jetson.md) - - Otherwise, install from [Linux Debian Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) - - In this case treat yourself as a developer: make sure to follow the instructions to also install librealsense2-dev and librealsense2-dkms packages - -- #### Option 2: Build from source - - Download the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.51.1) +
+ + +

+ Installation +

+ +
+ + Step 1: Install the ROS2 distribution + + +- #### Ubuntu 22.04: + - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) +- #### Ubuntu 20.04: + - [ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) + - [ROS2 Galactic](https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html) +- #### Ubuntu 18.04 : + - [ROS2 Dashing](https://docs.ros.org/en/dashing/Installation/Ubuntu-Install-Debians.html) + - [ROS2 Eloquent](https://docs.ros.org/en/eloquent/Installation/Linux-Install-Debians.html) + +
+ +
+ + Step 2: Install latest Intel® RealSense™ SDK 2.0 + + +- #### Option 1: Install librealsense2 debian package from Intel servers + - Jetson users - use the [Jetson Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_jetson.md) + - Otherwise, install from [Linux Debian Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) + - In this case treat yourself as a developer: make sure to follow the instructions to also install librealsense2-dev and librealsense2-dkms packages + +- #### Option 2: Install librealsense2 (without graphical tools and examples) debian package from ROS servers: + - [Configure](http://wiki.ros.org/Installation/Ubuntu/Sources) your Ubuntu repositories + - Install all realsense ROS packages by ```sudo apt install ros--librealsense2*``` + - For example, for Humble distro: ```sudo apt install ros-humble-librealsense2*``` + +- #### Option 3: Build from source + - Download the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.53.1) - Follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) - -### Step 3: Install Intel® RealSense™ ROS2 wrapper from sources - - Create a ROS2 workspace +
+ +
+ + Step 3: Install Intel® RealSense™ ROS2 wrapper + + +#### Option 1: Install debian package from ROS servers + - [Configure](http://wiki.ros.org/Installation/Ubuntu/Sources) your Ubuntu repositories + - Install all realsense ROS packages by ```sudo apt install ros--realsense2-*``` + - For example, for Humble distro: ```sudo apt install ros-humble-realsense2-*``` + +#### Option 2: Install from source + + - Create a ROS2 workspace ```bash mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src/ ``` - - Clone the latest ROS2 Intel® RealSense™ wrapper from [here](https://github.com/IntelRealSense/realsense-ros.git) into '~/ros2_ws/src/' + + - Clone the latest ROS2 Intel® RealSense™ wrapper from [here](https://github.com/IntelRealSense/realsense-ros.git) into '~/ros2_ws/src/' ```bashrc git clone https://github.com/IntelRealSense/realsense-ros.git -b ros2-development cd ~/ros2_ws ``` -### Step 4: Install dependencies + + - Install dependencies ```bash sudo apt-get install python3-rosdep -y sudo rosdep init # "sudo rosdep init --include-eol-distros" for Eloquent and earlier @@ -72,43 +134,98 @@ LibRealSense supported version: v2.51.1 (see [realsense2_camera release notes](h rosdep install -i --from-path src --rosdistro $ROS_DISTRO --skip-keys=librealsense2 -y ``` -### Step 5: Build + - Build ```bash colcon build ``` -### Step 6: Terminal environment + - Source environment ```bash ROS_DISTRO= # set your ROS_DISTRO: humble, galactic, foxy, eloquent, dashing source /opt/ros/$ROS_DISTRO/setup.bash cd ~/ros2_ws . install/local_setup.bash ``` + +
+ +

+ Usage +

+ +

+ Start the camera node +

+ + #### with ros2 run: + ros2 run realsense2_camera realsense2_camera_node + # or, with parameters, for example - temporal and spatial filters are enabled: + ros2 run realsense2_camera realsense2_camera_node --ros-args -p enable_color:=false -p spatial_filter.enable:=true -p temporal_filter.enable:=true + + #### with ros2 launch: + ros2 launch realsense2_camera rs_launch.py + ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true + +

+ Parameters +

+ +### Sensor Parameters: +Each sensor has a unique set of parameters. +Video sensors, such as depth_module or rgb_camera have, at least, the 'profile' parameter.
+It is a string of the following format: \X\X\ (The deviding character can be X, x or ",". Spaces are ignored.) -  +Since infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by their common sensor. +If the specified combination of parameters is not available by the device, the default configuration will be used.
-## Usage Instructions +### Available Parameters: +For the entire list of parameters type `ros2 param list`. +For reading a parameter value use `ros2 param get ` for instance: `ros2 param get /camera/camera depth_module.emitter_on_off` +For setting a new value for a parameter use `ros2 param set ` i.e. `ros2 param set /camera/camera depth_module.emitter_on_off true` -### Start the camera node -To start the camera node in ROS: +#### Parameters that can be modified during runtime: +- All of the filters and sensors inner parameters. +- **enable_****: Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams. can be any of *infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*. +- **enable_sync**: gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag. This happens automatically when such filters as pointcloud are enabled. +- ****_qos**: can be any of *infra, color, fisheye, depth, gyro, accel, pose*. Sets the QoS by which the topic is published. Available values are the following strings: SYSTEM_DEFAULT, DEFAULT, PARAMETER_EVENTS, SERVICES_DEFAULT, PARAMETERS, SENSOR_DATA. +- **Notice:** ****_info_qos** refers to both camera_info topics and metadata topics. +- **tf_publish_rate**: double, positive values mean dynamic transform publication with specified rate, all other values mean static transform publication. Defaults to 0 -```bash -ros2 launch realsense2_camera rs_launch.py -``` -or, with parameters, for example - temporal and spatial filters are enabled: -```bash -ros2 run realsense2_camera realsense2_camera_node --ros-args -p enable_color:=false -p spatial_filter.enable:=true -p temporal_filter.enable:=true -``` +#### Parameters that cannot be changed in runtime: +- **serial_no**: will attach to the device with the given serial number (*serial_no*) number. Default, attach to the first (in an inner list) RealSense device. + - Note: serial number can also be defined with "_" prefix. For instance, serial number 831612073525 can be set in command line as `serial_no:=_831612073525`. That is a workaround until a better method will be found to ROS2's auto conversion of strings containing only digits into integers. +- **usb_port_id**: will attach to the device with the given USB port (*usb_port_id*). i.e 4-1, 4-2 etc. Default, ignore USB port when choosing a device. +- **device_type**: will attach to a device whose name includes the given *device_type* regular expression pattern. Default, ignore device type. For example, device_type:=d435 will match d435 and d435i. device_type=d435(?!i) will match d435 but not d435i. +- **reconnect_timeout**: When the driver cannot connect to the device try to reconnect after this timeout (in seconds). +- **wait_for_device_timeout**: If the specified device is not found, will wait *wait_for_device_timeout* seconds before exits. Defualt, *wait_for_device_timeout < 0*, will wait indefinitely. +- **rosbag_filename**: Publish topics from rosbag file. There are two ways for loading rosbag file: + * Command line - ```ros2 run realsense2_camera realsense2_camera_node -p rosbag_filename:="/full/path/to/rosbag.bag"``` + * Launch file - set ```rosbag_filename``` parameter with rosbag full path (see ```realsense2_camera/launch/rs_launch.py``` as reference) -or, with a launch file: -```bash -ros2 launch realsense2_camera rs_launch.py -ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true -``` +- **initial_reset**: On occasions the device was not closed properly and due to firmware issues needs to reset. If set to true, the device will reset prior to usage. -This will stream all camera sensors and publish on the appropriate ROS topics. +- ****_frame_id**, ****_optical_frame_id**, **aligned_depth_to_**_frame_id**: Specify the different frame_id for the different frames. Especially important when using multiple cameras. -### Published Topics +- **base_frame_id**: defines the frame_id all static transformations refers to. +- **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. + +- **unite_imu_method**: The D435i camera has built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. +Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. The *imu* topic is published at the rate of the gyro. All the fields of the Imu message under the *imu* topic are filled out. `unite_imu_method` parameter supported values are [0-2] meaning: [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when: + - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. + - **copy**: Every gyro message is attached by the last accel message. +- **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) +- **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. +- **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. +- **publish_tf**: boolean, publish or not TF at all. Defaults to True. +- **diagnostics_period**: double, positive values set the period between diagnostics updates on the `/diagnostics` topic. 0 or negative values mean no diagnostics topic is published. Defaults to 0.
+The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. + +- **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame. + +

+ Published Topics +

+ The published topics differ according to the device and parameters. After running the above command with D435i attached, the following list of topics will be available (This is a partial list. For full one type `ros2 topic list`): - /camera/aligned_depth_to_color/camera_info @@ -127,8 +244,9 @@ After running the above command with D435i attached, the following list of topic - /rosout - /tf_static +This will stream relevant camera sensors and publish on the appropriate ROS topics. -Enabling accel and gyro is achieved either by adding the following parameters to the command line:
+ Enabling accel and gyro is achieved either by adding the following parameters to the command line:
`ros2 launch realsense2_camera rs_launch.py pointcloud.enable:=true enable_gyro:=true enable_accel:=true`
or in runtime using the following commands: ``` @@ -146,117 +264,57 @@ Enabling stream adds matching topics. For instance, enabling the gyro and accel - /camera/gyro/metadata - /camera/gyro/sample - ->Using an L515 device the list differs a little by adding a 4-bit confidence grade (published as a mono8 image): ->- /camera/confidence/camera_info ->- /camera/confidence/image_rect_raw -> ->It also replaces the 2 infrared topic sets with the single available one: ->- /camera/infra/camera_info ->- /camera/infra/image_raw - -To turn them off: `ros2 param set /camera/camera enable_infra false` -The "/camera" prefix is the namesapce specified in the given launch file. -When using D435 or D415, the gyro and accel topics wont be available. - -### The metadata topic: +

+ Metadata topic +

+ The metadata messages store the camera's available metadata in a *json* format. To learn more, a dedicated script for echoing a metadata topic in runtime is attached. For instance, use the following command to echo the camera/depth/metadata topic: ``` python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/depth/metadata ``` - -### Post processing blocks - i.e. filters: -The following processing blocks are available: + +

+ Post-Processing Filters +

+ +The following post processing filters are available: - ```align_depth```: If enabled, will publish the depth image aligned to the color image on the topic `/camera/aligned_depth_to_color/image_raw`.
The pointcloud, if created, will be based on the aligned depth image. - - ```colorizer```: will color the depth image. On the depth topic an RGB image will be published, instead of the 16bit depth values . - ```pointcloud```: will add a pointcloud topic `/camera/depth/color/points`. * The texture of the pointcloud can be modified using the `pointcloud.stream_filter` parameter.
* The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `pointcloud.allow_no_texture_points` to true. * pointcloud is of an unordered format by default. This can be changed by setting `pointcloud.ordered_pc` to true. - - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`.
To view the effect on the infrared image for each sequence id use the `sequence_id_filter.sequence_id` parameter.
To initialize these parameters in start time use the following parameters:
`depth_module.exposure.1`, `depth_module.gain.1`, `depth_module.exposure.2`, `depth_module.gain.2`
\* For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). - - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md - - ```disparity_filter``` - convert depth to disparity before applying other filters and back. - - ```spatial_filter``` - filter the depth image spatially. - - ```temporal_filter``` - filter the depth image temporally. - - ```hole_filling_filter``` - apply hole-filling filter. - - ```decimation_filter``` - reduces depth scene complexity. + - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md + - ```disparity_filter``` - convert depth to disparity before applying other filters and back. + - ```spatial_filter``` - filter the depth image spatially. + - ```temporal_filter``` - filter the depth image temporally. + - ```hole_filling_filter``` - apply hole-filling filter. + - ```decimation_filter``` - reduces depth scene complexity. Each of the above filters have it's own parameters, following the naming convention of `.` including a `.enable` parameter to enable/disable it. -### Sensor Parameters: -Each sensor has a unique set of parameters. -Video sensors, such as depth_module or rgb_camera have, at least, the 'profile' parameter.
-It is a string of the following format: \X\X\ (The deviding character can be X, x or ",". Spaces are ignored.) - -Since infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by their common sensor. -The same rule applies in L515 for the depth, infra and confidence streams which all share the parameters of their common depth_module. -If the specified combination of parameters is not available by the device, the default configuration will be used.
- -### Available Parameters: -For the entire list of parameters type `ros2 param list`. -For reading a parameter value use `ros2 param get ` for instance: `ros2 param get /camera/camera depth_module.emitter_on_off` -For setting a new value for a parameter use `ros2 param set ` i.e. `ros2 param set /camera/camera depth_module.emitter_on_off true` - -#### Parameters that can be modified during runtime: -- All of the filters and sensors inner parameters. -- **enable_****: Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams. can be any of *infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*. -- **enable_sync**: gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag. This happens automatically when such filters as pointcloud are enabled. -- ****_qos**: can be any of *infra, color, fisheye, depth, gyro, accel, pose*. Sets the QoS by which the topic is published. Available values are the following strings: SYSTEM_DEFAULT, DEFAULT, PARAMETER_EVENTS, SERVICES_DEFAULT, PARAMETERS, SENSOR_DATA. -- **Notice:** ****_info_qos** refers to both camera_info topics and metadata topics. -- **tf_publish_rate**: double, positive values mean dynamic transform publication with specified rate, all other values mean static transform publication. Defaults to 0 - - - -#### Parameters that cannot be changed in runtime: -- **serial_no**: will attach to the device with the given serial number (*serial_no*) number. Default, attach to the first (in an inner list) RealSense device. - - Note: serial number can also be defined with "_" prefix. For instance, serial number 831612073525 can be set in command line as `serial_no:=_831612073525`. That is a workaround until a better method will be found to ROS2's auto conversion of strings containing only digits into integers. -- **usb_port_id**: will attach to the device with the given USB port (*usb_port_id*). i.e 4-1, 4-2 etc. Default, ignore USB port when choosing a device. -- **device_type**: will attach to a device whose name includes the given *device_type* regular expression pattern. Default, ignore device type. For example, device_type:=d435 will match d435 and d435i. device_type=d435(?!i) will match d435 but not d435i. -- **reconnect_timeout**: When the driver cannot connect to the device try to reconnect after this timeout (in seconds). -- **wait_for_device_timeout**: If the specified device is not found, will wait *wait_for_device_timeout* seconds before exits. Defualt, *wait_for_device_timeout < 0*, will wait indefinitely. -- **rosbag_filename**: Publish topics from rosbag file. There are two ways for loading rosbag file: - * Command line - ```ros2 run realsense2_camera realsense2_camera_node -p rosbag_filename:="/full/path/to/rosbag.bag"``` - * Launch file - set ```rosbag_filename``` parameter with rosbag full path (see ```realsense2_camera/launch/rs_launch.py``` as reference) - -- **initial_reset**: On occasions the device was not closed properly and due to firmware issues needs to reset. If set to true, the device will reset prior to usage. - -- ****_frame_id**, ****_optical_frame_id**, **aligned_depth_to_**_frame_id**: Specify the different frame_id for the different frames. Especially important when using multiple cameras. - -- **base_frame_id**: defines the frame_id all static transformations refers to. -- **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. - -- **unite_imu_method**: The D435i camera has built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. -Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. The *imu* topic is published at the rate of the gyro. All the fields of the Imu message under the *imu* topic are filled out. `unite_imu_method` parameter supported values are [0-2] meaning: [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when: - - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. - - **copy**: Every gyro message is attached by the last accel message. -- **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) -- **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. -- **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. -- **publish_tf**: boolean, publish or not TF at all. Defaults to True. -- **diagnostics_period**: double, positive values set the period between diagnostics updates on the `/diagnostics` topic. 0 or negative values mean no diagnostics topic is published. Defaults to 0.
-The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. - -- **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame. - -### Available services: +

+ Available services +

+ - device_info : retrieve information about the device - serial_number, firmware_version etc. Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. Call example: `ros2 service call /camera/device_info realsense2_camera_msgs/srv/DeviceInfo` - Note that for **ROS2 Dashing** the command is `ros2 srv show realsense2_camera_msgs/srv/DeviceInfo` - -## Efficient intra-process communication: - +

+ Efficient intra-process communication: +

+ Our ROS2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can reduce copy times on image topics (not point-cloud or others), especially with big frame resolutions and high FPS. You will need to launch a component container and launch our node as a component together with other component nodes. Further details on "Composing multiple nodes in a single process" can be found [here](https://docs.ros.org/en/rolling/Tutorials/Composition.html). Further details on efficient intra-process communication can be found [here](https://docs.ros.org/en/foxy/Tutorials/Intra-Process-Communication.html#efficient-intra-process-communication). -### Example + ### Example #### Manually loading multiple components into the same process * Start the component: ```bash @@ -290,37 +348,277 @@ The launch file accepts a parameter, `intra_process_comms`, controlling whether ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comms:=true ``` +
+ +

+ Contributing +

+ + # How to Contribute -## Still in the pipeline: +This project welcomes third-party code via GitHub pull requests. +You are welcome to propose and discuss enhancements using project [issues](https://github.com/IntelRealSense/realsense-ros/issues). -* Migrate infra_rgb option. +> **Branching Policy**: +> The `ros2-development` branch is considered stable, at all times. +> If you plan to propose a patch, please commit into the `ros2-development` branch, or its own feature branch. -### Unit tests: -Unit-tests are based on bag files saved on S3 server. These can be downloaded using the following commands: -```bash -cd ros2_ws -wget "http://realsense-hw-public.s3.amazonaws.com/rs-tests/TestData/outdoors.bag" -P "records/" -wget "http://realsense-hw-public.s3-eu-west-1.amazonaws.com/rs-tests/D435i_Depth_and_IMU_Stands_still.bag" -P "records/" -``` -Then, unit-tests can be run using the following command (use either python or python3): -```bash -python3 src/realsense-ros/realsense2_camera/scripts/rs2_test.py --all -``` +In addition, please run `pr_check.sh` under `scripts` directory. This scripts verify compliance with project's standards: -## License -Copyright 2022 Intel Corporation +1. Every example / source file must refer to [LICENSE](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/LICENSE) +2. Every example / source file must include correct copyright notice +3. For indentation we are using spaces and not tabs +4. Line-endings must be Unix and not DOS style -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this project except in compliance with the License. -You may obtain a copy of the License at +Most common issues can be automatically resolved by running `./pr_check.sh --fix` - http://www.apache.org/licenses/LICENSE-2.0 +Please familirize yourself with the [Apache License 2.0](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/LICENSE) before contributing. -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. +## Step-by-Step -**Other names and brands may be claimed as the property of others* +1. Make sure you have `git` and `cmake` installed on your system. On Windows we recommend using [Git Extensions](https://github.com/gitextensions/gitextensions/releases) for git bash. +2. Run `git clone https://github.com/IntelRealSense/realsense-ros.git` and `cd realsense-ros` +3. To align with latest status of the ros2-development branch, run: +``` +git fetch origin +git checkout ros2-development +git reset --hard origin/ros2-development +``` +4. `git checkout -b name_of_your_contribution` to create a dedicated branch +5. Make your changes to the local repository +6. Make sure your local git user is updated, or run `git config --global user.email "email@example.com"` and `git config --global user.user "user"` to set it up. This is the user & email that will appear in GitHub history. +7. `git add -p` to select the changes you wish to add +8. `git commit -m "Description of the change"` +9. Make sure you have a GitHub user and [fork realsense-ros](https://github.com/IntelRealSense/realsense-ros#fork-destination-box) +10. `git remote add fork https://github.com/username/realsense-ros.git` with your GitHub `username` +11. `git fetch fork` +12. `git push fork` to push `name_of_your_contribution` branch to your fork +13. Go to your fork on GitHub at `https://github.com/username/realsense-ros` +14. Click the `New pull request` button +15. For `base` combo-box select `ros2-development`, since you want to submit a PR to that branch +16. For `compare` combo-box select `name_of_your_contribution` with your commit +17. Review your changes and click `Create pull request` +18. Wait for all automated checks to pass +19. The PR will be approved / rejected after review from the team and the community + +To continue to new change, goto step 3. +To return to your PR (in order to make more changes): +1. `git stash` +2. `git checkout name_of_your_contribution` +3. Repeat items 5-8 from the previous list +4. `git push fork` +The pull request will be automatically updated + +
+ +
+ +

+ License +

+ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + 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. + +
From 929fbc5128ab6ce425b617acece7d5adea5e4ec3 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Sun, 26 Mar 2023 13:08:32 +0300 Subject: [PATCH 48/85] use playback device for rosbags --- realsense2_camera/src/realsense_node_factory.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 92984b8590..1e90ce131a 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -281,12 +281,8 @@ void RealSenseNodeFactory::init() { { ROS_INFO_STREAM("publish topics from rosbag file: " << rosbag_filename.c_str()); - auto pipe = std::make_shared(); - rs2::config cfg; - cfg.enable_device_from_file(rosbag_filename.c_str(), false); - cfg.enable_all_streams(); - pipe->start(cfg); //File will be opened in read mode at this point - _device = pipe->get_active_profile().get_device(); + rs2::context ctx; + _device = ctx.load_device(rosbag_filename.c_str()); _serial_no = _device.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); } if (_device) From ec8023a55af2bedccba07805d91e0b59a336ae5d Mon Sep 17 00:00:00 2001 From: Arun Prasad V Date: Thu, 30 Mar 2023 19:19:43 +0530 Subject: [PATCH 49/85] Avoid configuring dynamic_tf_broadcaster within tf_publish_rate param's callback --- realsense2_camera/include/base_realsense_node.h | 2 +- realsense2_camera/src/base_realsense_node.cpp | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 2047994a45..a4ce0ea567 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -261,7 +261,7 @@ namespace realsense2_camera std::mutex _publish_dynamic_tf_mutex; std::shared_ptr _static_tf_broadcaster; - std::shared_ptr _dynamic_tf_broadcaster; + std::shared_ptr _dynamic_tf_broadcaster = std::make_shared(_node); std::vector _static_tf_msgs; std::shared_ptr _tf_t; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 8056d44d7a..cb68970e09 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -989,7 +989,6 @@ void BaseRealSenseNode::startDynamicTf() ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate); if (!_tf_t) { - _dynamic_tf_broadcaster = std::make_shared(_node); _tf_t = std::make_shared([this]() { publishDynamicTransforms(); @@ -1002,7 +1001,6 @@ void BaseRealSenseNode::startDynamicTf() { _tf_t->join(); _tf_t.reset(); - _dynamic_tf_broadcaster.reset(); } } } From b092f35138de9feb9b9db559303aac6560c6f8d8 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 31 Mar 2023 01:48:38 +0530 Subject: [PATCH 50/85] Avoid configuring dynamic_tf_broadcaster within tf_publish_rate's cb --- realsense2_camera/include/base_realsense_node.h | 2 +- realsense2_camera/src/base_realsense_node.cpp | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index a4ce0ea567..2047994a45 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -261,7 +261,7 @@ namespace realsense2_camera std::mutex _publish_dynamic_tf_mutex; std::shared_ptr _static_tf_broadcaster; - std::shared_ptr _dynamic_tf_broadcaster = std::make_shared(_node); + std::shared_ptr _dynamic_tf_broadcaster; std::vector _static_tf_msgs; std::shared_ptr _tf_t; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index cb68970e09..3ba3470907 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1001,12 +1001,18 @@ void BaseRealSenseNode::startDynamicTf() { _tf_t->join(); _tf_t.reset(); + _dynamic_tf_broadcaster.reset(); } } } void BaseRealSenseNode::publishDynamicTransforms() { + if (!_dynamic_tf_broadcaster) + { + _dynamic_tf_broadcaster = std::make_shared(_node); + } + // Publish transforms for the cameras std::unique_lock lock(_publish_dynamic_tf_mutex); while (rclcpp::ok() && _is_running && _tf_publish_rate > 0) From f032edb009022e0219463121c1b53820aa23655e Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 4 Apr 2023 00:06:45 +0530 Subject: [PATCH 51/85] Fix: Don't publish /tf when publish_tf is false --- realsense2_camera/src/base_realsense_node.cpp | 25 +++++++++++++------ realsense2_camera/src/parameters.cpp | 1 - 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 3ba3470907..ae095cfa2d 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -984,16 +984,22 @@ void BaseRealSenseNode::publishStaticTransforms(std::vector void BaseRealSenseNode::startDynamicTf() { + if (!_publish_tf) + { + ROS_WARN("Since the param 'publish_tf' is set to 'false'," + "the value set on the param 'tf_publish_rate' won't have any effect"); + return; + } if (_tf_publish_rate > 0) { - ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate); - if (!_tf_t) - { - _tf_t = std::make_shared([this]() + ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate); + if (!_tf_t) { - publishDynamicTransforms(); - }); - } + _tf_t = std::make_shared([this]() + { + publishDynamicTransforms(); + }); + } } else { @@ -1002,6 +1008,11 @@ void BaseRealSenseNode::startDynamicTf() _tf_t->join(); _tf_t.reset(); _dynamic_tf_broadcaster.reset(); + ROS_WARN("Stopped publishing dynamic camera transforms (/tf)"); + } + else + { + ROS_WARN("Currently not publishing dynamic camera transforms (/tf)"); } } } diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 053704c067..3a2ef1c77c 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -37,7 +37,6 @@ void BaseRealSenseNode::getParameters() startDynamicTf(); }); _parameters_names.push_back(param_name); - startDynamicTf(); param_name = std::string("diagnostics_period"); _diagnostics_period = _parameters->setParam(param_name, DIAGNOSTICS_PERIOD); From 1e38de70811f45b9b14c2558aa0096ee185892d1 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 4 Apr 2023 00:23:00 +0530 Subject: [PATCH 52/85] Indentation corrections --- realsense2_camera/src/base_realsense_node.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index ae095cfa2d..66b231b2a4 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -992,14 +992,14 @@ void BaseRealSenseNode::startDynamicTf() } if (_tf_publish_rate > 0) { - ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate); - if (!_tf_t) + ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate); + if (!_tf_t) + { + _tf_t = std::make_shared([this]() { - _tf_t = std::make_shared([this]() - { - publishDynamicTransforms(); - }); - } + publishDynamicTransforms(); + }); + } } else { From ce5002717828ea0157a0b86b1ed24eac0097c87d Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 5 Apr 2023 20:45:37 +0530 Subject: [PATCH 53/85] Added 'publish_tf' param in rs launch files --- realsense2_camera/launch/rs_intra_process_demo_launch.py | 2 ++ realsense2_camera/launch/rs_launch.py | 1 + 2 files changed, 3 insertions(+) diff --git a/realsense2_camera/launch/rs_intra_process_demo_launch.py b/realsense2_camera/launch/rs_intra_process_demo_launch.py index 6c240b5e2a..5ffee703be 100644 --- a/realsense2_camera/launch/rs_intra_process_demo_launch.py +++ b/realsense2_camera/launch/rs_intra_process_demo_launch.py @@ -54,6 +54,8 @@ {'name': 'enable_gyro', 'default': 'false', 'description': "enable gyro stream"}, {'name': 'enable_accel', 'default': 'false', 'description': "enable accel stream"}, {'name': 'intra_process_comms', 'default': 'true', 'description': "enable intra-process communication"}, + {'name': 'publish_tf', 'default': 'true', 'description': 'enable/disable publishing tf'}, + {'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'}, ] def declare_configurable_parameters(parameters): diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index fdb6c2789c..07530bd7ff 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -58,6 +58,7 @@ {'name': 'initial_reset', 'default': 'false', 'description': "''"}, {'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"}, {'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''}, + {'name': 'publish_tf', 'default': 'true', 'description': 'enable/disable publishing tf'}, {'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'}, {'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'}, {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'}, From 81b49eecc72061fd74ce43783c49af67e75a9e86 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 5 Apr 2023 19:14:11 +0300 Subject: [PATCH 54/85] invert translation --- realsense2_camera/src/base_realsense_node.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 8056d44d7a..babe643733 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -865,9 +865,13 @@ void BaseRealSenseNode::publish_static_tf(const rclcpp::Time& t, msg.header.stamp = t; msg.header.frame_id = from; msg.child_frame_id = to; + + // Convert x,y,z (taken from camera extrinsics) + // from optical cooridnates to ros coordinates msg.transform.translation.x = trans.z; msg.transform.translation.y = -trans.x; msg.transform.translation.z = -trans.y; + msg.transform.rotation.x = q.getX(); msg.transform.rotation.y = q.getY(); msg.transform.rotation.z = q.getZ(); @@ -917,7 +921,10 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& auto Q = rotationMatrixToQuaternion(ex.rotation); Q = quaternion_optical * Q * quaternion_optical.inverse(); - float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]}; + // Invert x,y,z of extrinsic translation before sending this as TF translation + // because extrinsic from A to B is the position of A relative to B + // while TF from A to B is the transformation to be done on A to get to B + float3 trans{-ex.translation[0], -ex.translation[1], -ex.translation[2]}; publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); // Transform stream frame to stream optical frame From b520dc1926eea6d431710cb929a20bdd4152f169 Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Thu, 6 Apr 2023 01:03:31 +0300 Subject: [PATCH 55/85] Remove ubuntu 18.04 from main workflow --- .github/workflows/main.yml | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 695909a32f..58d5ebe7f4 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -23,7 +23,7 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [rolling, humble, galactic, foxy, eloquent, dashing] + ros_distro: [rolling, humble, galactic, foxy] include: - ros_distro: 'rolling' os: ubuntu-22.04 @@ -33,10 +33,6 @@ jobs: os: ubuntu-20.04 - ros_distro: 'foxy' os: ubuntu-20.04 - - ros_distro: 'eloquent' - os: ubuntu-18.04 - - ros_distro: 'dashing' - os: ubuntu-18.04 steps: @@ -63,12 +59,12 @@ jobs: with: cmake-version: '3.23.0' - ## If distro is one of [galactic, foxy, eloquent, dashing], use the setup-ros@v0.2 + ## If distro is one of [galactic, foxy], use the setup-ros@v0.2 ## which supports old versions of ROS2 and EOL versions. ## For Humble distro, use v0.3 ## See: 1) https://github.com/ros-tooling/setup-ros#Supported-platforms ## 2) https://github.com/ros-tooling/setup-ros/tree/v0.2#Supported-platforms - - name: build ROS2 Galactic/Foxy/Eloquent/Dashing + - name: build ROS2 Galactic/Foxy if: ${{ matrix.ros_distro != 'humble' && matrix.ros_distro != 'rolling'}} uses: ros-tooling/setup-ros@v0.2 with: From e130f9ff4d329f8684d9609074ff5fb630b7a7ce Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 6 Apr 2023 01:29:33 +0300 Subject: [PATCH 56/85] remove license and contributing content from the README.md, but point this to relative files --- README.md | 319 ++++++++---------------------------------------------- 1 file changed, 44 insertions(+), 275 deletions(-) diff --git a/README.md b/README.md index 55d2b7498a..841d076e36 100644 --- a/README.md +++ b/README.md @@ -4,12 +4,30 @@

ROS2 packages for using Intel RealSense D400 cameras.
- Supported ROS2 Distros: Dashing, Eloquent, Foxy, Humble and Rolling.
Latest release notes

+
+ +[![rolling][rolling-badge]][rolling] +[![humble][humble-badge]][humble] +[![foxy][foxy-badge]][foxy] +[![galactic][galactic-badge]][galactic] +[![eloquent][eloquent-badge]][eloquent] +[![dashing][dashing-badge]][dashing] +[![ubuntu22][ubuntu22-badge]][ubuntu22] +[![ubuntu20][ubuntu20-badge]][ubuntu20] +[![ubuntu18][ubuntu18-badge]][ubuntu18] + +![GitHubWorkflowStatus](https://img.shields.io/github/actions/workflow/status/IntelRealSense/realsense-ros/main.yml?logo=github&style=flat-square) +![GitHubcontributors](https://img.shields.io/github/contributors/IntelRealSense/realsense-ros?style=flat-square) +![License](https://img.shields.io/github/license/IntelRealSense/realsense-ros?style=flat-square) + +
+ + ## Table of contents * [ROS1 and ROS2 legacy](#legacy) * [Installation](#installation) @@ -21,8 +39,8 @@ * [Post-Processing Filters](#filters) * [Available Services](#services) * [Efficient intra-process communication](#intra-process) - * [Contributing](#contributing) - * [License](#license) + * [Contributing](CONTRIBUTING.md) + * [License](LICENSE)

Legacy @@ -348,277 +366,28 @@ The launch file accepts a parameter, `intra_process_comms`, controlling whether ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comms:=true ``` -
- -

- Contributing -

- - # How to Contribute - -This project welcomes third-party code via GitHub pull requests. - -You are welcome to propose and discuss enhancements using project [issues](https://github.com/IntelRealSense/realsense-ros/issues). - -> **Branching Policy**: -> The `ros2-development` branch is considered stable, at all times. -> If you plan to propose a patch, please commit into the `ros2-development` branch, or its own feature branch. - -In addition, please run `pr_check.sh` under `scripts` directory. This scripts verify compliance with project's standards: - -1. Every example / source file must refer to [LICENSE](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/LICENSE) -2. Every example / source file must include correct copyright notice -3. For indentation we are using spaces and not tabs -4. Line-endings must be Unix and not DOS style - -Most common issues can be automatically resolved by running `./pr_check.sh --fix` - -Please familirize yourself with the [Apache License 2.0](https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/LICENSE) before contributing. - -## Step-by-Step - -1. Make sure you have `git` and `cmake` installed on your system. On Windows we recommend using [Git Extensions](https://github.com/gitextensions/gitextensions/releases) for git bash. -2. Run `git clone https://github.com/IntelRealSense/realsense-ros.git` and `cd realsense-ros` -3. To align with latest status of the ros2-development branch, run: -``` -git fetch origin -git checkout ros2-development -git reset --hard origin/ros2-development -``` -4. `git checkout -b name_of_your_contribution` to create a dedicated branch -5. Make your changes to the local repository -6. Make sure your local git user is updated, or run `git config --global user.email "email@example.com"` and `git config --global user.user "user"` to set it up. This is the user & email that will appear in GitHub history. -7. `git add -p` to select the changes you wish to add -8. `git commit -m "Description of the change"` -9. Make sure you have a GitHub user and [fork realsense-ros](https://github.com/IntelRealSense/realsense-ros#fork-destination-box) -10. `git remote add fork https://github.com/username/realsense-ros.git` with your GitHub `username` -11. `git fetch fork` -12. `git push fork` to push `name_of_your_contribution` branch to your fork -13. Go to your fork on GitHub at `https://github.com/username/realsense-ros` -14. Click the `New pull request` button -15. For `base` combo-box select `ros2-development`, since you want to submit a PR to that branch -16. For `compare` combo-box select `name_of_your_contribution` with your commit -17. Review your changes and click `Create pull request` -18. Wait for all automated checks to pass -19. The PR will be approved / rejected after review from the team and the community - -To continue to new change, goto step 3. -To return to your PR (in order to make more changes): -1. `git stash` -2. `git checkout name_of_your_contribution` -3. Repeat items 5-8 from the previous list -4. `git push fork` -The pull request will be automatically updated -
-
- -

- License -

- - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - 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. - -
+ +[supporteddistros-badge]: https://img.shields.io/badge/-Supported%20ROS2%20Distros-lightgrey?style=flat-square&logo=ros +[supporteddistro]: https://docs.ros.org/en/rolling/Releases.html +[rolling-badge]: https://img.shields.io/badge/-ROLLING-blue?style=flat-square&logo=ros +[rolling]: https://docs.ros.org/en/rolling/Releases.html +[humble-badge]: https://img.shields.io/badge/-HUMBLE-blue?style=flat-square&logo=ros +[humble]: https://docs.ros.org/en/rolling/Releases/Release-Humble-Hawksbill.html +[foxy-badge]: https://img.shields.io/badge/-FOXY-blue?style=flat-square&logo=ros +[foxy]: https://docs.ros.org/en/rolling/Releases/Release-Foxy-Fitzroy.html +[galactic-badge]: https://img.shields.io/badge/-GALACTIC-blue?style=flat-square&logo=ros +[galactic]: https://docs.ros.org/en/galactic/Releases/Release-Foxy-Fitzroy.html +[eloquent-badge]: https://img.shields.io/badge/-ELOQUENT-blue?style=flat-square&logo=ros +[eloquent]: https://docs.ros.org/en/eloquent/Releases/Release-Foxy-Fitzroy.html +[dashing-badge]: https://img.shields.io/badge/-DASHING-blue?style=flat-square&logo=ros +[dashing]: https://docs.ros.org/en/dashing/Releases/Release-Foxy-Fitzroy.html +[supported-ubuntu-badge]: https://img.shields.io/badge/-Supported%20Ubuntu%20Releases-lightgrey?style=flat-square&logo=ubuntu&logoColor=white +[supported-ubuntu]: https://wiki.ubuntu.com/Releases +[ubuntu22-badge]: https://img.shields.io/badge/-UBUNTU%2022%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white +[ubuntu22]: https://releases.ubuntu.com/jammy/ +[ubuntu20-badge]: https://img.shields.io/badge/-UBUNTU%2020%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white +[ubuntu20]: https://releases.ubuntu.com/focal/ +[ubuntu18-badge]: https://img.shields.io/badge/-UBUNTU%2018%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white +[ubuntu18]: https://releases.ubuntu.com/18.04/ \ No newline at end of file From c7bdec8ce190012a3f9cffe881d137326fa18f00 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Fri, 7 Apr 2023 00:41:33 +0300 Subject: [PATCH 57/85] remove ubuntu 18.04 and eloquent and dashing from readme --- README.md | 106 ++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 84 insertions(+), 22 deletions(-) diff --git a/README.md b/README.md index 841d076e36..577eabb2e0 100644 --- a/README.md +++ b/README.md @@ -9,23 +9,19 @@
-
[![rolling][rolling-badge]][rolling] [![humble][humble-badge]][humble] [![foxy][foxy-badge]][foxy] [![galactic][galactic-badge]][galactic] -[![eloquent][eloquent-badge]][eloquent] -[![dashing][dashing-badge]][dashing] [![ubuntu22][ubuntu22-badge]][ubuntu22] [![ubuntu20][ubuntu20-badge]][ubuntu20] -[![ubuntu18][ubuntu18-badge]][ubuntu18] ![GitHubWorkflowStatus](https://img.shields.io/github/actions/workflow/status/IntelRealSense/realsense-ros/main.yml?logo=github&style=flat-square) ![GitHubcontributors](https://img.shields.io/github/contributors/IntelRealSense/realsense-ros?style=flat-square) ![License](https://img.shields.io/github/license/IntelRealSense/realsense-ros?style=flat-square) -
+
## Table of contents @@ -34,6 +30,12 @@ * [Usage](#usage) * [Starting the camera node](#start-camera-node) * [Parameters](#parameters) + * [ROS2-vs-Optical Coordination Systems](#coordination) + * [Point Of View](#point-of-view) + * [ROS (Robot) Coordinates](#ros-coordination) + * [Optical (Camera) Coordinates](#optical-coordination) + * [TFs](#tfs) + * [Extrinsics](#extrinsics) * [Topics](#topics) * [Metadata Topic](#metadata) * [Post-Processing Filters](#filters) @@ -42,6 +44,8 @@ * [Contributing](CONTRIBUTING.md) * [License](LICENSE) +
+

Legacy

@@ -93,9 +97,6 @@ - #### Ubuntu 20.04: - [ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) - [ROS2 Galactic](https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html) -- #### Ubuntu 18.04 : - - [ROS2 Dashing](https://docs.ros.org/en/dashing/Installation/Ubuntu-Install-Debians.html) - - [ROS2 Eloquent](https://docs.ros.org/en/eloquent/Installation/Linux-Install-Debians.html) @@ -159,7 +160,7 @@ - Source environment ```bash - ROS_DISTRO= # set your ROS_DISTRO: humble, galactic, foxy, eloquent, dashing + ROS_DISTRO= # set your ROS_DISTRO: humble, galactic, foxy source /opt/ros/$ROS_DISTRO/setup.bash cd ~/ros2_ws . install/local_setup.bash @@ -167,6 +168,8 @@ +
+

Usage

@@ -183,7 +186,9 @@ #### with ros2 launch: ros2 launch realsense2_camera rs_launch.py ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true - + +
+

Parameters

@@ -239,7 +244,67 @@ Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. - **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame. - + +
+ + +

+ ROS2(Robot) vs Optical(Camera) Coordination Systems: +

+ +

+ Point Of View: +

+ +- Imagine we are standing behind of the camera, and looking forward. +- Always use this point of view when talking about coordinates, left vs right IRs, position of sensor, etc.. + +

+ ROS2 Coordinate System: +

+ +ROS2 Coordinate System (X: Forward, Y:Left, Z: Up) + +

+ Camera Optical Coordinate System: +

+ +Camera Optical Coordinate System (X: Right, Y: Down, Z: Forward). + +

+ TF from coordinate A to coordinate B: +

+ +- TF msg expresses a transform from coordinate frame "header.frame_id" (source) to the coordinate frame child_frame_id (destination) [Reference](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Transform.html) +- In RealSense cameras, the origin point (0,0,0) is taken from the left IR (infra1) position and named as "camera_link" frame +- Depth, left IR and "camera_link" coordinates converge together. +- Our wrapper provide TFs between each sensor coordinate to the camera base (camera_link) +- Also, it provides TFs from each sensor ROS coordinates to its corrosponding optical coordinates. + +

+ Extrinsics from sensor A to sensor B: +

+ +- Extrinsic from sensor A to sensor B means the position and orientation of sensor A relative to sensor B. +- Imagine that B is the origin (0,0,0), then the Extrensics(A->B) describes where is sensor A relative to sensor B. +- For example, depth_to_color, in D435i: + - If we look from behind of the D435i, extrinsic from depth to color, means, where is the depth in relative to the color. + - If we just look at the X coordinates, in the optical coordiantes (again, from behind) and assume that COLOR(RGB) sensor is (0,0,0), we can say that DEPTH sensor is on the right of RGB by 0.0148m (1.48cm). + +``` +administrator@perclnx466 ~/ros2_humble $ ros2 topic echo /camera/extrinsics/depth_to_color +rotation: +... +translation: +- 0.01485931035131216 +- 0.0010161789832636714 +- 0.0005317096947692335 +--- +``` +![image](https://user-images.githubusercontent.com/99127997/230220297-e392f0fc-63bf-4bab-8001-af1ddf0ed00e.png) + +
+

Published Topics

@@ -282,6 +347,8 @@ Enabling stream adds matching topics. For instance, enabling the gyro and accel - /camera/gyro/metadata - /camera/gyro/sample +
+

Metadata topic

@@ -291,6 +358,8 @@ The metadata messages store the camera's available metadata in a *json* format. python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/depth/metadata ``` +
+

Post-Processing Filters

@@ -315,12 +384,15 @@ The following post processing filters are available: Each of the above filters have it's own parameters, following the naming convention of `.` including a `.enable` parameter to enable/disable it. +
+

Available services

- device_info : retrieve information about the device - serial_number, firmware_version etc. Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list. Call example: `ros2 service call /camera/device_info realsense2_camera_msgs/srv/DeviceInfo` - - Note that for **ROS2 Dashing** the command is `ros2 srv show realsense2_camera_msgs/srv/DeviceInfo` + +

Efficient intra-process communication: @@ -369,8 +441,6 @@ ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comm -[supporteddistros-badge]: https://img.shields.io/badge/-Supported%20ROS2%20Distros-lightgrey?style=flat-square&logo=ros -[supporteddistro]: https://docs.ros.org/en/rolling/Releases.html [rolling-badge]: https://img.shields.io/badge/-ROLLING-blue?style=flat-square&logo=ros [rolling]: https://docs.ros.org/en/rolling/Releases.html [humble-badge]: https://img.shields.io/badge/-HUMBLE-blue?style=flat-square&logo=ros @@ -379,15 +449,7 @@ ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comm [foxy]: https://docs.ros.org/en/rolling/Releases/Release-Foxy-Fitzroy.html [galactic-badge]: https://img.shields.io/badge/-GALACTIC-blue?style=flat-square&logo=ros [galactic]: https://docs.ros.org/en/galactic/Releases/Release-Foxy-Fitzroy.html -[eloquent-badge]: https://img.shields.io/badge/-ELOQUENT-blue?style=flat-square&logo=ros -[eloquent]: https://docs.ros.org/en/eloquent/Releases/Release-Foxy-Fitzroy.html -[dashing-badge]: https://img.shields.io/badge/-DASHING-blue?style=flat-square&logo=ros -[dashing]: https://docs.ros.org/en/dashing/Releases/Release-Foxy-Fitzroy.html -[supported-ubuntu-badge]: https://img.shields.io/badge/-Supported%20Ubuntu%20Releases-lightgrey?style=flat-square&logo=ubuntu&logoColor=white -[supported-ubuntu]: https://wiki.ubuntu.com/Releases [ubuntu22-badge]: https://img.shields.io/badge/-UBUNTU%2022%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white [ubuntu22]: https://releases.ubuntu.com/jammy/ [ubuntu20-badge]: https://img.shields.io/badge/-UBUNTU%2020%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white [ubuntu20]: https://releases.ubuntu.com/focal/ -[ubuntu18-badge]: https://img.shields.io/badge/-UBUNTU%2018%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white -[ubuntu18]: https://releases.ubuntu.com/18.04/ \ No newline at end of file From 42fb45abef43eeed1da9e3cfb6d6a952f642ab00 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Fri, 7 Apr 2023 01:55:24 +0300 Subject: [PATCH 58/85] rearrange --- README.md | 179 ++++++++++++++++++++++++++++++++++-------------------- 1 file changed, 112 insertions(+), 67 deletions(-) diff --git a/README.md b/README.md index 577eabb2e0..e0d519af5d 100644 --- a/README.md +++ b/README.md @@ -18,12 +18,11 @@ [![ubuntu20][ubuntu20-badge]][ubuntu20] ![GitHubWorkflowStatus](https://img.shields.io/github/actions/workflow/status/IntelRealSense/realsense-ros/main.yml?logo=github&style=flat-square) -![GitHubcontributors](https://img.shields.io/github/contributors/IntelRealSense/realsense-ros?style=flat-square) -![License](https://img.shields.io/github/license/IntelRealSense/realsense-ros?style=flat-square) +[![GitHubcontributors](https://img.shields.io/github/contributors/IntelRealSense/realsense-ros?style=flat-square)](CONTRIBUTING.md) +[![License](https://img.shields.io/github/license/IntelRealSense/realsense-ros?style=flat-square)](LICENSE)
- ## Table of contents * [ROS1 and ROS2 legacy](#legacy) * [Installation](#installation) @@ -31,11 +30,8 @@ * [Starting the camera node](#start-camera-node) * [Parameters](#parameters) * [ROS2-vs-Optical Coordination Systems](#coordination) - * [Point Of View](#point-of-view) - * [ROS (Robot) Coordinates](#ros-coordination) - * [Optical (Camera) Coordinates](#optical-coordination) - * [TFs](#tfs) - * [Extrinsics](#extrinsics) + * [TF from coordinate A to coordinate B](#tfs) + * [Extrinsics from sensor A to sensor B](#extrinsics) * [Topics](#topics) * [Metadata Topic](#metadata) * [Post-Processing Filters](#filters) @@ -194,96 +190,136 @@

### Sensor Parameters: -Each sensor has a unique set of parameters. -Video sensors, such as depth_module or rgb_camera have, at least, the 'profile' parameter.
-It is a string of the following format: \X\X\ (The deviding character can be X, x or ",". Spaces are ignored.) +- Each sensor has a unique set of parameters. +- Video sensors, such as depth_module or rgb_camera have, at least, the 'profile' parameter.
+ - The profile parameter is a string of the following format: \X\X\ (The deviding character can be X, x or ",". Spaces are ignored.) + - For example: ```depth_module.profile:=640x480x30``` +- Since infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by their common sensor. +- If the specified combination of parameters is not available by the device, the default configuration will be used. -Since infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by their common sensor. -If the specified combination of parameters is not available by the device, the default configuration will be used.
+
### Available Parameters: -For the entire list of parameters type `ros2 param list`. -For reading a parameter value use `ros2 param get ` for instance: `ros2 param get /camera/camera depth_module.emitter_on_off` -For setting a new value for a parameter use `ros2 param set ` i.e. `ros2 param set /camera/camera depth_module.emitter_on_off true` +- For the entire list of parameters type `ros2 param list`. +- For reading a parameter value use `ros2 param get ` + - For example: `ros2 param get /camera/camera depth_module.emitter_on_off` +- For setting a new value for a parameter use `ros2 param set ` + - For example: `ros2 param set /camera/camera depth_module.emitter_on_off true` #### Parameters that can be modified during runtime: - All of the filters and sensors inner parameters. -- **enable_****: Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams. can be any of *infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*. -- **enable_sync**: gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag. This happens automatically when such filters as pointcloud are enabled. -- ****_qos**: can be any of *infra, color, fisheye, depth, gyro, accel, pose*. Sets the QoS by which the topic is published. Available values are the following strings: SYSTEM_DEFAULT, DEFAULT, PARAMETER_EVENTS, SERVICES_DEFAULT, PARAMETERS, SENSOR_DATA. +- **enable_****: + - Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams. + - can be any of *infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*. + - For example: ```enable_infra1:=true enable_color:=false``` +- **enable_sync**: + - gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag. + - This happens automatically when such filters as pointcloud are enabled. +- ****_qos**: + - Sets the QoS by which the topic is published. + - can be any of *infra, color, fisheye, depth, gyro, accel, pose*. + - Available values are the following strings: `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA`. + - For example: ```depth_qos:=SENSOR_DATA``` + - Reference: [ROS2 QoS profiles formal documentation](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles) - **Notice:** ****_info_qos** refers to both camera_info topics and metadata topics. - **tf_publish_rate**: double, positive values mean dynamic transform publication with specified rate, all other values mean static transform publication. Defaults to 0 #### Parameters that cannot be changed in runtime: -- **serial_no**: will attach to the device with the given serial number (*serial_no*) number. Default, attach to the first (in an inner list) RealSense device. - - Note: serial number can also be defined with "_" prefix. For instance, serial number 831612073525 can be set in command line as `serial_no:=_831612073525`. That is a workaround until a better method will be found to ROS2's auto conversion of strings containing only digits into integers. -- **usb_port_id**: will attach to the device with the given USB port (*usb_port_id*). i.e 4-1, 4-2 etc. Default, ignore USB port when choosing a device. -- **device_type**: will attach to a device whose name includes the given *device_type* regular expression pattern. Default, ignore device type. For example, device_type:=d435 will match d435 and d435i. device_type=d435(?!i) will match d435 but not d435i. -- **reconnect_timeout**: When the driver cannot connect to the device try to reconnect after this timeout (in seconds). -- **wait_for_device_timeout**: If the specified device is not found, will wait *wait_for_device_timeout* seconds before exits. Defualt, *wait_for_device_timeout < 0*, will wait indefinitely. -- **rosbag_filename**: Publish topics from rosbag file. There are two ways for loading rosbag file: +- **serial_no**: + - will attach to the device with the given serial number (*serial_no*) number. + - Default, attach to the first (in an inner list) RealSense device. + - Note: serial number should be defined with "_" prefix. + - That is a workaround until a better method will be found to ROS2's auto conversion of strings containing only digits into integers. + - Example: serial number 831612073525 can be set in command line as `serial_no:=_831612073525`. +- **usb_port_id**: + - will attach to the device with the given USB port (*usb_port_id*). + - For example: `usb_port_id:=4-1` or `usb_port_id:=4-2` + - Default, ignore USB port when choosing a device. +- **device_type**: + - will attach to a device whose name includes the given *device_type* regular expression pattern. + - Default, ignore device type. + - For example: + - `device_type:=d435` will match d435 and d435i. + - `device_type=d435(?!i)` will match d435 but not d435i. +- **reconnect_timeout**: + - When the driver cannot connect to the device try to reconnect after this timeout (in seconds). + - For Example: `reconnect_timeout:=10` +- **wait_for_device_timeout**: + - If the specified device is not found, will wait *wait_for_device_timeout* seconds before exits. + - Defualt, *wait_for_device_timeout < 0*, will wait indefinitely. + - For example: `wait_for_device_timeout:=60` +- **rosbag_filename**: + - Publish topics from rosbag file. There are two ways for loading rosbag file: * Command line - ```ros2 run realsense2_camera realsense2_camera_node -p rosbag_filename:="/full/path/to/rosbag.bag"``` * Launch file - set ```rosbag_filename``` parameter with rosbag full path (see ```realsense2_camera/launch/rs_launch.py``` as reference) - -- **initial_reset**: On occasions the device was not closed properly and due to firmware issues needs to reset. If set to true, the device will reset prior to usage. - +- **initial_reset**: + - On occasions the device was not closed properly and due to firmware issues needs to reset. + - If set to true, the device will reset prior to usage. + - For example: `initial_reset:=true` - ****_frame_id**, ****_optical_frame_id**, **aligned_depth_to_**_frame_id**: Specify the different frame_id for the different frames. Especially important when using multiple cameras. - - **base_frame_id**: defines the frame_id all static transformations refers to. - **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. -- **unite_imu_method**: The D435i camera has built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. -Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. The *imu* topic is published at the rate of the gyro. All the fields of the Imu message under the *imu* topic are filled out. `unite_imu_method` parameter supported values are [0-2] meaning: [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when: - - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. - - **copy**: Every gyro message is attached by the last accel message. -- **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) +- **unite_imu_method**: + - D400 cameras have built in IMU components which produce 2 unrelated streams, each with it's own frequency: + - *gyro* - which shows angular velocity + - *accel* which shows linear acceleration. + - By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. + - Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. + - The *imu* topic is published at the rate of the gyro. + - All the fields of the Imu message under the *imu* topic are filled out. + - `unite_imu_method` parameter supported values are [0-2] meaning: [0 -> None, 1 -> Copy, 2 -> Linear_ interpolation] when: + - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. + - **copy**: Every gyro message is attached by the last accel message. +- **clip_distance**: + - Remove from the depth image all values above a given value (meters). Disable by giving negative value (default) + - For example: `clip_distance:=1.5` - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. -- **publish_tf**: boolean, publish or not TF at all. Defaults to True. -- **diagnostics_period**: double, positive values set the period between diagnostics updates on the `/diagnostics` topic. 0 or negative values mean no diagnostics topic is published. Defaults to 0.
+- **publish_tf**: + - boolean, publish or not TF at all. + - Defaults to True. +- **diagnostics_period**: + - double, positive values set the period between diagnostics updates on the `/diagnostics` topic. + - 0 or negative values mean no diagnostics topic is published. Defaults to 0.
The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams. - **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame.
-

ROS2(Robot) vs Optical(Camera) Coordination Systems:

-

- Point Of View: -

- -- Imagine we are standing behind of the camera, and looking forward. -- Always use this point of view when talking about coordinates, left vs right IRs, position of sensor, etc.. - -

- ROS2 Coordinate System: -

+- Point Of View: + - Imagine we are standing behind of the camera, and looking forward. + - Always use this point of view when talking about coordinates, left vs right IRs, position of sensor, etc.. -ROS2 Coordinate System (X: Forward, Y:Left, Z: Up) +![image](https://user-images.githubusercontent.com/99127997/230150735-bc31fedf-d715-4e35-b462-fe2c338832c3.png) -

- Camera Optical Coordinate System: -

+- ROS2 Coordinate System: (X: Forward, Y:Left, Z: Up) +- Camera Optical Coordinate System: (X: Right, Y: Down, Z: Forward) -Camera Optical Coordinate System (X: Right, Y: Down, Z: Forward). +
-

+

TF from coordinate A to coordinate B: -

+

- TF msg expresses a transform from coordinate frame "header.frame_id" (source) to the coordinate frame child_frame_id (destination) [Reference](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Transform.html) - In RealSense cameras, the origin point (0,0,0) is taken from the left IR (infra1) position and named as "camera_link" frame - Depth, left IR and "camera_link" coordinates converge together. -- Our wrapper provide TFs between each sensor coordinate to the camera base (camera_link) +- Our wrapper provide static TFs between each sensor coordinate to the camera base (camera_link) - Also, it provides TFs from each sensor ROS coordinates to its corrosponding optical coordinates. +- Example of static TFs of RGB sensor and Infra2 (right infra) sensor of D435i module as it shown in rviz2: +![example](https://user-images.githubusercontent.com/99127997/230148106-0f79cbdb-c401-4d09-b386-a366af18e5f7.png) + +
-

+

Extrinsics from sensor A to sensor B: -

+

- Extrinsic from sensor A to sensor B means the position and orientation of sensor A relative to sensor B. - Imagine that B is the origin (0,0,0), then the Extrensics(A->B) describes where is sensor A relative to sensor B. @@ -291,6 +327,8 @@ Camera Optical Coordinate System (X: Right, Y: Down, Z: Forward). - If we look from behind of the D435i, extrinsic from depth to color, means, where is the depth in relative to the color. - If we just look at the X coordinates, in the optical coordiantes (again, from behind) and assume that COLOR(RGB) sensor is (0,0,0), we can say that DEPTH sensor is on the right of RGB by 0.0148m (1.48cm). +![d435i](https://user-images.githubusercontent.com/99127997/230220297-e392f0fc-63bf-4bab-8001-af1ddf0ed00e.png) + ``` administrator@perclnx466 ~/ros2_humble $ ros2 topic echo /camera/extrinsics/depth_to_color rotation: @@ -301,7 +339,7 @@ translation: - 0.0005317096947692335 --- ``` -![image](https://user-images.githubusercontent.com/99127997/230220297-e392f0fc-63bf-4bab-8001-af1ddf0ed00e.png) +
@@ -365,15 +403,22 @@ python3 src/realsense-ros/realsense2_camera/scripts/echo_metadada.py /camera/dep

The following post processing filters are available: - - ```align_depth```: If enabled, will publish the depth image aligned to the color image on the topic `/camera/aligned_depth_to_color/image_raw`.
The pointcloud, if created, will be based on the aligned depth image. + - ```align_depth```: If enabled, will publish the depth image aligned to the color image on the topic `/camera/aligned_depth_to_color/image_raw`. + - The pointcloud, if created, will be based on the aligned depth image. - ```colorizer```: will color the depth image. On the depth topic an RGB image will be published, instead of the 16bit depth values . - ```pointcloud```: will add a pointcloud topic `/camera/depth/color/points`. * The texture of the pointcloud can be modified using the `pointcloud.stream_filter` parameter.
* The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `pointcloud.allow_no_texture_points` to true. * pointcloud is of an unordered format by default. This can be changed by setting `pointcloud.ordered_pc` to true. - - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`.
To view the effect on the infrared image for each sequence id use the `sequence_id_filter.sequence_id` parameter.
To initialize these parameters in start time use the following parameters:
- `depth_module.exposure.1`, `depth_module.gain.1`, `depth_module.exposure.2`, `depth_module.gain.2`
- \* For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). + - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. + - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. + - To view the effect on the infrared image for each sequence id use the `sequence_id_filter.sequence_id` parameter. + - To initialize these parameters in start time use the following parameters: + - `depth_module.exposure.1` + - `depth_module.gain.1` + - `depth_module.exposure.2` + - `depth_module.gain.2` + - For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md - ```disparity_filter``` - convert depth to disparity before applying other filters and back. @@ -441,13 +486,13 @@ ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comm -[rolling-badge]: https://img.shields.io/badge/-ROLLING-blue?style=flat-square&logo=ros +[rolling-badge]: https://img.shields.io/badge/-ROLLING-orange?style=flat-square&logo=ros [rolling]: https://docs.ros.org/en/rolling/Releases.html -[humble-badge]: https://img.shields.io/badge/-HUMBLE-blue?style=flat-square&logo=ros +[humble-badge]: https://img.shields.io/badge/-HUMBLE-orange?style=flat-square&logo=ros [humble]: https://docs.ros.org/en/rolling/Releases/Release-Humble-Hawksbill.html -[foxy-badge]: https://img.shields.io/badge/-FOXY-blue?style=flat-square&logo=ros +[foxy-badge]: https://img.shields.io/badge/-FOXY-orange?style=flat-square&logo=ros [foxy]: https://docs.ros.org/en/rolling/Releases/Release-Foxy-Fitzroy.html -[galactic-badge]: https://img.shields.io/badge/-GALACTIC-blue?style=flat-square&logo=ros +[galactic-badge]: https://img.shields.io/badge/-GALACTIC-orange?style=flat-square&logo=ros [galactic]: https://docs.ros.org/en/galactic/Releases/Release-Foxy-Fitzroy.html [ubuntu22-badge]: https://img.shields.io/badge/-UBUNTU%2022%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white [ubuntu22]: https://releases.ubuntu.com/jammy/ From 5a3f9a4c01e585d29596b031acdc4de4bba1c0c6 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 10 Apr 2023 00:18:54 +0300 Subject: [PATCH 59/85] add deprecated msg to galactic --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index e0d519af5d..cfeace86aa 100644 --- a/README.md +++ b/README.md @@ -92,7 +92,7 @@ - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) - #### Ubuntu 20.04: - [ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) - - [ROS2 Galactic](https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html) + - [ROS2 Galactic](https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html) (deprecated) From a1a8255181b9caa825d18590ba09cb18ab4285ab Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 10 Apr 2023 00:24:02 +0300 Subject: [PATCH 60/85] add summary/details pans for TFs and Extrinsics --- README.md | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index cfeace86aa..614bc84473 100644 --- a/README.md +++ b/README.md @@ -303,9 +303,12 @@ The `/diagnostics` topic includes information regarding the device temperatures
-

- TF from coordinate A to coordinate B: -

+
+ +

+ TF from coordinate A to coordinate B: +

+ - TF msg expresses a transform from coordinate frame "header.frame_id" (source) to the coordinate frame child_frame_id (destination) [Reference](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Transform.html) - In RealSense cameras, the origin point (0,0,0) is taken from the left IR (infra1) position and named as "camera_link" frame @@ -315,11 +318,17 @@ The `/diagnostics` topic includes information regarding the device temperatures - Example of static TFs of RGB sensor and Infra2 (right infra) sensor of D435i module as it shown in rviz2: ![example](https://user-images.githubusercontent.com/99127997/230148106-0f79cbdb-c401-4d09-b386-a366af18e5f7.png) +
+
-

- Extrinsics from sensor A to sensor B: -

+
+ +

+ Extrinsics from sensor A to sensor B: +

+ + - Extrinsic from sensor A to sensor B means the position and orientation of sensor A relative to sensor B. - Imagine that B is the origin (0,0,0), then the Extrensics(A->B) describes where is sensor A relative to sensor B. @@ -340,6 +349,7 @@ translation: --- ``` +
From 1e08819664926111c5df3096bfef3dfa4db78ffa Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 10 Apr 2023 01:49:56 +0300 Subject: [PATCH 61/85] fix matrix rotation --- .../include/base_realsense_node.h | 1 + realsense2_camera/src/base_realsense_node.cpp | 38 ++++++++++++++++--- 2 files changed, 33 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 2047994a45..b794380332 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -205,6 +205,7 @@ namespace realsense2_camera void publishDynamicTransforms(); void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset); Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const; + rs2_extrinsics invertExtrinsics(const rs2_extrinsics& ex) const; IMUInfo getImuInfo(const rs2::stream_profile& profile); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index babe643733..ad36a2d343 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -888,6 +888,30 @@ void BaseRealSenseNode::publishExtrinsicsTopic(const stream_index_pair& sip, con } } + +rs2_extrinsics BaseRealSenseNode::invertExtrinsics(const rs2_extrinsics& ex) const +{ + rs2_extrinsics result; + + // invert translation vector + result.translation[0] = -ex.translation[0]; + result.translation[1] = -ex.translation[1]; + result.translation[2] = -ex.translation[2]; + + // invert rotation matrix + result.rotation[0] = ex.rotation[0]; + result.rotation[1] = ex.rotation[3]; + result.rotation[2] = ex.rotation[6]; + result.rotation[3] = ex.rotation[1]; + result.rotation[4] = ex.rotation[4]; + result.rotation[5] = ex.rotation[7]; + result.rotation[6] = ex.rotation[2]; + result.rotation[7] = ex.rotation[5]; + result.rotation[8] = ex.rotation[8]; + + return result; +} + void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile) { // Transform base to stream @@ -918,13 +942,17 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& } } - auto Q = rotationMatrixToQuaternion(ex.rotation); - Q = quaternion_optical * Q * quaternion_optical.inverse(); + publishExtrinsicsTopic(sip, ex); - // Invert x,y,z of extrinsic translation before sending this as TF translation + // Invert extrinsic translation vector and rotation matrix before sending this as TF translation // because extrinsic from A to B is the position of A relative to B // while TF from A to B is the transformation to be done on A to get to B - float3 trans{-ex.translation[0], -ex.translation[1], -ex.translation[2]}; + ex = invertExtrinsics(ex); + + auto Q = rotationMatrixToQuaternion(ex.rotation); + Q = quaternion_optical * Q * quaternion_optical.inverse(); + float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]}; + publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); // Transform stream frame to stream optical frame @@ -941,8 +969,6 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& publish_static_tf(transform_ts_, zero_trans, zero_rot_quaternions, FRAME_ID(sip), IMU_FRAME_ID); publish_static_tf(transform_ts_, zero_trans, quaternion_optical, IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); } - - publishExtrinsicsTopic(sip, ex); } void BaseRealSenseNode::SetBaseStream() From d171559cc941a94ce22885e1553619def3b97edb Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 10 Apr 2023 11:14:24 +0300 Subject: [PATCH 62/85] calculate extriniscs twice in two opposite ways to save inverting rotation matrix --- .../include/base_realsense_node.h | 1 - realsense2_camera/src/base_realsense_node.cpp | 56 ++++++------------- 2 files changed, 18 insertions(+), 39 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index b794380332..2047994a45 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -205,7 +205,6 @@ namespace realsense2_camera void publishDynamicTransforms(); void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset); Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const; - rs2_extrinsics invertExtrinsics(const rs2_extrinsics& ex) const; IMUInfo getImuInfo(const rs2::stream_profile& profile); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index ad36a2d343..743fb23f9c 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -888,30 +888,6 @@ void BaseRealSenseNode::publishExtrinsicsTopic(const stream_index_pair& sip, con } } - -rs2_extrinsics BaseRealSenseNode::invertExtrinsics(const rs2_extrinsics& ex) const -{ - rs2_extrinsics result; - - // invert translation vector - result.translation[0] = -ex.translation[0]; - result.translation[1] = -ex.translation[1]; - result.translation[2] = -ex.translation[2]; - - // invert rotation matrix - result.rotation[0] = ex.rotation[0]; - result.rotation[1] = ex.rotation[3]; - result.rotation[2] = ex.rotation[6]; - result.rotation[3] = ex.rotation[1]; - result.rotation[4] = ex.rotation[4]; - result.rotation[5] = ex.rotation[7]; - result.rotation[6] = ex.rotation[2]; - result.rotation[7] = ex.rotation[5]; - result.rotation[8] = ex.rotation[8]; - - return result; -} - void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile) { // Transform base to stream @@ -924,17 +900,25 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& rclcpp::Time transform_ts_ = _node.now(); - rs2_extrinsics ex; + // extrinsic from A to B is the position of A relative to B + // TF from A to B is the transformation to be done on A to get to B + // so, we need to calculate extrinsics in two opposite ways, one for extrinsic topic + // and the second is for transformation topic (TF) + rs2_extrinsics normal_ex; // used to for extrinsics topic + rs2_extrinsics tf_ex; // used for TF + try { - ex = base_profile.get_extrinsics_to(profile); + normal_ex = base_profile.get_extrinsics_to(profile); + tf_ex = profile.get_extrinsics_to(base_profile); } catch (std::exception& e) { if (!strcmp(e.what(), "Requested extrinsics are not available!")) { - ROS_WARN_STREAM("(" << rs2_stream_to_string(profile.stream_type()) << ", " << profile.stream_index() << ") -> (" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << "): " << e.what() << " : using unity as default."); - ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); + ROS_WARN_STREAM("(" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << ") -> (" << rs2_stream_to_string(profile.stream_type()) << ", " << profile.stream_index() << "): " << e.what() << " : using unity as default."); + normal_ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); + tf_ex = normal_ex; } else { @@ -942,20 +926,16 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& } } - publishExtrinsicsTopic(sip, ex); - - // Invert extrinsic translation vector and rotation matrix before sending this as TF translation - // because extrinsic from A to B is the position of A relative to B - // while TF from A to B is the transformation to be done on A to get to B - ex = invertExtrinsics(ex); + // publish normal extrinsics e.g. /camera/extrinsics/depth_to_color + publishExtrinsicsTopic(sip, normal_ex); - auto Q = rotationMatrixToQuaternion(ex.rotation); + // publish static TF + auto Q = rotationMatrixToQuaternion(tf_ex.rotation); Q = quaternion_optical * Q * quaternion_optical.inverse(); - float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]}; - + float3 trans{tf_ex.translation[0], tf_ex.translation[1], tf_ex.translation[2]}; publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); - // Transform stream frame to stream optical frame + // Transform stream frame to stream optical frame and publish it publish_static_tf(transform_ts_, zero_trans, quaternion_optical, FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); if (profile.is() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1) From 48b362b2151fbce1a781a2bc774e9a077b3490d9 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 10 Apr 2023 13:05:57 +0300 Subject: [PATCH 63/85] remove collapses from TFs and Extrinsics, and edit links of ROS2 Distros --- README.md | 30 ++++++++++-------------------- 1 file changed, 10 insertions(+), 20 deletions(-) diff --git a/README.md b/README.md index 614bc84473..4be3949c30 100644 --- a/README.md +++ b/README.md @@ -303,12 +303,9 @@ The `/diagnostics` topic includes information regarding the device temperatures
-
- -

- TF from coordinate A to coordinate B: -

- +

+ TF from coordinate A to coordinate B: +

- TF msg expresses a transform from coordinate frame "header.frame_id" (source) to the coordinate frame child_frame_id (destination) [Reference](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Transform.html) - In RealSense cameras, the origin point (0,0,0) is taken from the left IR (infra1) position and named as "camera_link" frame @@ -318,16 +315,11 @@ The `/diagnostics` topic includes information regarding the device temperatures - Example of static TFs of RGB sensor and Infra2 (right infra) sensor of D435i module as it shown in rviz2: ![example](https://user-images.githubusercontent.com/99127997/230148106-0f79cbdb-c401-4d09-b386-a366af18e5f7.png) -
-
-
- -

- Extrinsics from sensor A to sensor B: -

- +

+ Extrinsics from sensor A to sensor B: +

- Extrinsic from sensor A to sensor B means the position and orientation of sensor A relative to sensor B. @@ -349,8 +341,6 @@ translation: --- ``` -
-

@@ -497,13 +487,13 @@ ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comm [rolling-badge]: https://img.shields.io/badge/-ROLLING-orange?style=flat-square&logo=ros -[rolling]: https://docs.ros.org/en/rolling/Releases.html +[rolling]: https://docs.ros.org/en/rolling/index.html [humble-badge]: https://img.shields.io/badge/-HUMBLE-orange?style=flat-square&logo=ros -[humble]: https://docs.ros.org/en/rolling/Releases/Release-Humble-Hawksbill.html +[humble]: https://docs.ros.org/en/humble/index.html [foxy-badge]: https://img.shields.io/badge/-FOXY-orange?style=flat-square&logo=ros -[foxy]: https://docs.ros.org/en/rolling/Releases/Release-Foxy-Fitzroy.html +[foxy]: https://docs.ros.org/en/foxy/index.html [galactic-badge]: https://img.shields.io/badge/-GALACTIC-orange?style=flat-square&logo=ros -[galactic]: https://docs.ros.org/en/galactic/Releases/Release-Foxy-Fitzroy.html +[galactic]: https://docs.ros.org/en/galactic/index.html [ubuntu22-badge]: https://img.shields.io/badge/-UBUNTU%2022%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white [ubuntu22]: https://releases.ubuntu.com/jammy/ [ubuntu20-badge]: https://img.shields.io/badge/-UBUNTU%2020%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white From 9d21262cba527391a004dc8074452dbde9ee3639 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 10 Apr 2023 13:09:25 +0300 Subject: [PATCH 64/85] add REP references for CS --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 4be3949c30..7bd8cf5625 100644 --- a/README.md +++ b/README.md @@ -300,6 +300,7 @@ The `/diagnostics` topic includes information regarding the device temperatures - ROS2 Coordinate System: (X: Forward, Y:Left, Z: Up) - Camera Optical Coordinate System: (X: Right, Y: Down, Z: Forward) +- References: [REP-0103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions) [REP-0105](https://www.ros.org/reps/rep-0105.html#coordinate-frames)
From ebacd30e2bbd70573cdce1b5ebc34fb07803181d Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Mon, 10 Apr 2023 17:21:59 +0300 Subject: [PATCH 65/85] update readme --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 7bd8cf5625..b13f2e0537 100644 --- a/README.md +++ b/README.md @@ -300,7 +300,9 @@ The `/diagnostics` topic includes information regarding the device temperatures - ROS2 Coordinate System: (X: Forward, Y:Left, Z: Up) - Camera Optical Coordinate System: (X: Right, Y: Down, Z: Forward) -- References: [REP-0103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions) [REP-0105](https://www.ros.org/reps/rep-0105.html#coordinate-frames) +- References: [REP-0103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions), [REP-0105](https://www.ros.org/reps/rep-0105.html#coordinate-frames) +- All data published in our wrapper topics is optical data taken directly from our camera sensors. +- static and dynamic TF topics publish optical CS and ROS CS to give the user the ability to move from one CS to other CS.
From 66647fb37da81a609da61fd4a9bc79dc0b752cec Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 11 Apr 2023 20:07:40 +0300 Subject: [PATCH 66/85] use rs2_to_ros to replace stereo module with depth moudle --- realsense2_camera/src/ros_sensor.cpp | 6 +++--- realsense2_camera/src/rs_node_setup.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 541b07bc04..d860a79dec 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -244,7 +244,7 @@ void RosSensor::stop() { if (get_active_streams().size() == 0) return; - ROS_INFO_STREAM("Stop Sensor: " << get_info(RS2_CAMERA_INFO_NAME)); + ROS_INFO_STREAM("Stop Sensor: " << rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); _frequency_diagnostics.clear(); try @@ -337,13 +337,13 @@ bool RosSensor::getUpdatedProfiles(std::vector& wanted_profiles) profile_manager->addWantedProfiles(wanted_profiles); } - ROS_DEBUG_STREAM(get_info(RS2_CAMERA_INFO_NAME) << ":" << "active_profiles.size() = " << active_profiles.size()); + ROS_DEBUG_STREAM(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME)) << ":" << "active_profiles.size() = " << active_profiles.size()); for (auto& profile : active_profiles) { ROS_DEBUG_STREAM("Sensor profile: " << ProfilesManager::profile_string(profile)); } - ROS_DEBUG_STREAM(get_info(RS2_CAMERA_INFO_NAME) << ":" << "wanted_profiles"); + ROS_DEBUG_STREAM(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME)) << ":" << "wanted_profiles"); for (auto& profile : wanted_profiles) { ROS_DEBUG_STREAM("Sensor profile: " << ProfilesManager::profile_string(profile)); diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 3648fc3c8a..b375a00de3 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -139,7 +139,7 @@ void BaseRealSenseNode::setAvailableSensors() for(auto&& sensor : _dev_sensors) { - const std::string module_name(sensor.get_info(RS2_CAMERA_INFO_NAME)); + const std::string module_name(rs2_to_ros(sensor.get_info(RS2_CAMERA_INFO_NAME))); std::unique_ptr rosSensor; if (sensor.is() || sensor.is() || @@ -304,7 +304,7 @@ void BaseRealSenseNode::updateSensors() try{ for(auto&& sensor : _available_ros_sensors) { - std::string module_name(sensor->get_info(RS2_CAMERA_INFO_NAME)); + std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))); // if active_profiles != wanted_profiles: stop sensor. std::vector wanted_profiles; @@ -386,7 +386,7 @@ void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceI for(auto&& sensor : _available_ros_sensors) { - sensors_names << create_graph_resource_name(sensor->get_info(RS2_CAMERA_INFO_NAME)) << ","; + sensors_names << create_graph_resource_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME))) << ","; } res->sensors = sensors_names.str().substr(0, sensors_names.str().size()-1); From c6914f0ddb78615d311ee14e0e291491b9c3af57 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 11 Apr 2023 21:18:27 +0300 Subject: [PATCH 67/85] clone PR1637 to ros2-development --- .../urdf/_materials.urdf.xacro | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/realsense2_description/urdf/_materials.urdf.xacro b/realsense2_description/urdf/_materials.urdf.xacro index 9a62789d8a..3f6d3eaeff 100644 --- a/realsense2_description/urdf/_materials.urdf.xacro +++ b/realsense2_description/urdf/_materials.urdf.xacro @@ -21,11 +21,15 @@ Collection of materials to be used in other macros. This avoids the redefinition of materials in case multple cameras are imported. --> - - - - - - - + + + + + + + + + + + From f36c390158327be80b5abeb06290bc4d215f1006 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 13 Apr 2023 00:25:35 +0530 Subject: [PATCH 68/85] Fix: Avoid /tf_static being broadcasted when 'publish_tf' param set to false --- realsense2_camera/src/base_realsense_node.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 66b231b2a4..75f55128f3 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -121,15 +121,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, ROS_INFO("Intra-Process communication enabled"); } - // intra-process do not support latched QoS, so we need to disable intra-process for this topic - rclcpp::PublisherOptionsWithAllocator> options; - options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - #ifndef DASHING - _static_tf_broadcaster = std::make_shared(node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); - #else - _static_tf_broadcaster = std::make_shared(node, rclcpp::QoS(100), std::move(options)); - #endif - _image_format[1] = CV_8UC1; // CVBridge type _image_format[2] = CV_16UC1; // CVBridge type _image_format[3] = CV_8UC3; // CVBridge type @@ -973,6 +964,19 @@ void BaseRealSenseNode::publishStaticTransforms(std::vector // Publish static transforms if (_publish_tf) { + if (!_static_tf_broadcaster) + { + // intra-process do not support latched QoS, so we need to disable intra-process for this topic + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + #ifndef DASHING + _static_tf_broadcaster = std::make_shared(_node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); + #else + _static_tf_broadcaster = std::make_shared(_node, rclcpp::QoS(100), std::move(options)); + #endif + } + for (auto &profile : profiles) { calcAndPublishStaticTransform(profile, _base_profile); From 5fc3c70d2f6013e692ec165e39970302ded08e27 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 13 Apr 2023 11:59:22 +0300 Subject: [PATCH 69/85] add info about extrinsic msg format in Extrinsics.msg and README.md --- README.md | 14 +++++++++++++- realsense2_camera_msgs/msg/Extrinsics.msg | 5 +++-- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index b13f2e0537..b7254e7ecc 100644 --- a/README.md +++ b/README.md @@ -336,7 +336,15 @@ The `/diagnostics` topic includes information regarding the device temperatures ``` administrator@perclnx466 ~/ros2_humble $ ros2 topic echo /camera/extrinsics/depth_to_color rotation: -... +- 0.9999583959579468 +- 0.008895332925021648 +- -0.0020127370953559875 +- -0.008895229548215866 +- 0.9999604225158691 +- 6.045500049367547e-05 +- 0.0020131953060626984 +- -4.254872692399658e-05 +- 0.9999979734420776 translation: - 0.01485931035131216 - 0.0010161789832636714 @@ -344,6 +352,10 @@ translation: --- ``` +- Extrinsic msg is made up of two parts: + - float64[9] rotation (Column - major 3x3 rotation matrix) + - float64[3] translation (Three-element translation vector, in meters) +

diff --git a/realsense2_camera_msgs/msg/Extrinsics.msg b/realsense2_camera_msgs/msg/Extrinsics.msg index 565aa97781..53c886e697 100644 --- a/realsense2_camera_msgs/msg/Extrinsics.msg +++ b/realsense2_camera_msgs/msg/Extrinsics.msg @@ -1,2 +1,3 @@ -float64[9] rotation -float64[3] translation +# Cross-stream extrinsics: encodes the topology describing how the different devices are oriented +float64[9] rotation # Column - major 3x3 rotation matrix +float64[3] translation # Three-element translation vector, in meters From 31f3cac95e81145e541686b91bbb9c550b22ad88 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 20 Apr 2023 04:48:18 +0530 Subject: [PATCH 70/85] Fixing /tf topic has only TFs of last started sensor --- .../include/base_realsense_node.h | 4 +- realsense2_camera/src/base_realsense_node.cpp | 71 +++++++++++++------ realsense2_camera/src/rs_node_setup.cpp | 32 ++++++++- 3 files changed, 82 insertions(+), 25 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 2047994a45..49ed525d43 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -165,6 +165,8 @@ namespace realsense2_camera const tf2::Quaternion& q, const std::string& from, const std::string& to); + void unpublish_static_tf(const std::string& frame_id, + const std::string& child_frame_id); void setup(); private: @@ -200,7 +202,7 @@ namespace realsense2_camera void updateExtrinsicsCalibData(const rs2::video_stream_profile& left_video_profile, const rs2::video_stream_profile& right_video_profile); void updateStreamCalibData(const rs2::video_stream_profile& video_profile); void SetBaseStream(); - void publishStaticTransforms(std::vector profiles); + void publishStaticTransforms(); void startDynamicTf(); void publishDynamicTransforms(); void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 75f55128f3..9025379c57 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -846,6 +846,37 @@ tf2::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotati return tf2::Quaternion(q.x(), q.y(), q.z(), q.w()); } +void BaseRealSenseNode::unpublish_static_tf(const std::string& frame_id, + const std::string& child_frame_id) +{ + std::vector::iterator it; + struct find_tf + { + const std::string& frame_id; + const std::string& child_frame_id; + + // Struct Constructor + find_tf(const std::string& frame_id, const std::string& child_frame_id) : + frame_id(frame_id), child_frame_id(child_frame_id) {} + + bool operator()(const geometry_msgs::msg::TransformStamped &static_tf_msg) const + { + return (static_tf_msg.header.frame_id == frame_id && + static_tf_msg.child_frame_id == child_frame_id); + } + }; + + // Find whether there is any TF with given 'frame_id' and 'child_frame_id' + it = std::find_if(_static_tf_msgs.begin(), _static_tf_msgs.end(), + find_tf(frame_id, child_frame_id)); + + // If found, erase that specific TF + if (it != std::end(_static_tf_msgs)) + { + _static_tf_msgs.erase(it); + } +} + void BaseRealSenseNode::publish_static_tf(const rclcpp::Time& t, const float3& trans, const tf2::Quaternion& q, @@ -959,31 +990,27 @@ void BaseRealSenseNode::SetBaseStream() _base_profile = available_profiles[*base_stream]; } -void BaseRealSenseNode::publishStaticTransforms(std::vector profiles) +void BaseRealSenseNode::publishStaticTransforms() { - // Publish static transforms - if (_publish_tf) + // Since the _static_tf_broadcaster is a latched topic, the old transforms will + // be alive even if the sensors are dynamically disabled. So, reset the + // broadcaster everytime and publish the transforms of enabled sensors alone. + if (_static_tf_broadcaster) { - if (!_static_tf_broadcaster) - { - // intra-process do not support latched QoS, so we need to disable intra-process for this topic - rclcpp::PublisherOptionsWithAllocator> options; - options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - - #ifndef DASHING - _static_tf_broadcaster = std::make_shared(_node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); - #else - _static_tf_broadcaster = std::make_shared(_node, rclcpp::QoS(100), std::move(options)); - #endif - } - - for (auto &profile : profiles) - { - calcAndPublishStaticTransform(profile, _base_profile); - } - if (_static_tf_broadcaster) - _static_tf_broadcaster->sendTransform(_static_tf_msgs); + _static_tf_broadcaster.reset(); } + + // intra-process do not support latched QoS, so we need to disable intra-process for this topic + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + #ifndef DASHING + _static_tf_broadcaster = std::make_shared(_node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); + #else + _static_tf_broadcaster = std::make_shared(_node, rclcpp::QoS(100), std::move(options)); + #endif + + _static_tf_broadcaster->sendTransform(_static_tf_msgs); } void BaseRealSenseNode::startDynamicTf() diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 3648fc3c8a..834546455b 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -195,6 +195,26 @@ void BaseRealSenseNode::stopPublishers(const std::vector& profil } _metadata_publishers.erase(sip); _extrinsics_publishers.erase(sip); + + if (_publish_tf) + { + std::lock_guard lock_guard(_publish_tf_mutex); + + unpublish_static_tf(_base_frame_id, FRAME_ID(sip)); + unpublish_static_tf(FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); + + if (profile.is() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1) + { + unpublish_static_tf(_base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); + unpublish_static_tf(ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); + } + + if ((_imu_sync_method > imu_sync_method::NONE) && (profile.stream_type() == RS2_STREAM_GYRO)) + { + unpublish_static_tf(FRAME_ID(sip), IMU_FRAME_ID); + unpublish_static_tf(IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); + } + } } } @@ -330,10 +350,13 @@ void BaseRealSenseNode::updateSensors() { startPublishers(wanted_profiles, *sensor); updateProfilesStreamCalibData(wanted_profiles); + if (_publish_tf) { std::lock_guard lock_guard(_publish_tf_mutex); - _static_tf_msgs.clear(); - publishStaticTransforms(wanted_profiles); + for (auto &profile : wanted_profiles) + { + calcAndPublishStaticTransform(profile, _base_profile); + } } if(is_profile_changed) @@ -351,6 +374,11 @@ void BaseRealSenseNode::updateSensors() } } } + if (_publish_tf) + { + std::lock_guard lock_guard(_publish_tf_mutex); + publishStaticTransforms(); + } } catch(const std::exception& ex) { From 7ebd9ce0268695e1f488ba3b46f499f221a90566 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 21 Apr 2023 19:08:00 +0530 Subject: [PATCH 71/85] Updated TF descriptions in launch py and readme --- README.md | 14 +++++++++++--- .../launch/rs_intra_process_demo_launch.py | 4 ++-- realsense2_camera/launch/rs_launch.py | 4 ++-- 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index b7254e7ecc..06ba97918f 100644 --- a/README.md +++ b/README.md @@ -222,7 +222,12 @@ - For example: ```depth_qos:=SENSOR_DATA``` - Reference: [ROS2 QoS profiles formal documentation](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles) - **Notice:** ****_info_qos** refers to both camera_info topics and metadata topics. -- **tf_publish_rate**: double, positive values mean dynamic transform publication with specified rate, all other values mean static transform publication. Defaults to 0 +- **tf_publish_rate**: + - double, rate (in Hz) at which dynamic transforms are published + - Default value is 0.0 Hz *(means no dynamic TF)* + - This param also depends on **publish_tf** param + - If **publish_tf:=false**, then no TFs will be published, even if **tf_publish_rate** is >0.0 Hz + - If **publish_tf:=true** and **tf_publish_rate** set to >0.0 Hz, then dynamic TFs will be published at the specified rate #### Parameters that cannot be changed in runtime: - **serial_no**: @@ -277,8 +282,11 @@ - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. - **publish_tf**: - - boolean, publish or not TF at all. - - Defaults to True. + - boolean, enable/disable publishing static and dynamic TFs + - Defaults to True + - So, static TFs will be published by default + - If dynamic TFs are needed, user should set the param **tf_publish_rate** to >0.0 Hz + - If set to false, both static and dynamic TFs won't be published, even if the param **tf_publish_rate** is set to >0.0 Hz - **diagnostics_period**: - double, positive values set the period between diagnostics updates on the `/diagnostics` topic. - 0 or negative values mean no diagnostics topic is published. Defaults to 0.
diff --git a/realsense2_camera/launch/rs_intra_process_demo_launch.py b/realsense2_camera/launch/rs_intra_process_demo_launch.py index 5ffee703be..103afee220 100644 --- a/realsense2_camera/launch/rs_intra_process_demo_launch.py +++ b/realsense2_camera/launch/rs_intra_process_demo_launch.py @@ -54,8 +54,8 @@ {'name': 'enable_gyro', 'default': 'false', 'description': "enable gyro stream"}, {'name': 'enable_accel', 'default': 'false', 'description': "enable accel stream"}, {'name': 'intra_process_comms', 'default': 'true', 'description': "enable intra-process communication"}, - {'name': 'publish_tf', 'default': 'true', 'description': 'enable/disable publishing tf'}, - {'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'}, + {'name': 'publish_tf', 'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'}, + {'name': 'tf_publish_rate', 'default': '0.0', 'description': '[double] rate in HZ for publishing dynamic TF'}, ] def declare_configurable_parameters(parameters): diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 07530bd7ff..db633ead9f 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -58,8 +58,8 @@ {'name': 'initial_reset', 'default': 'false', 'description': "''"}, {'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"}, {'name': 'pointcloud.ordered_pc', 'default': 'false', 'description': ''}, - {'name': 'publish_tf', 'default': 'true', 'description': 'enable/disable publishing tf'}, - {'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'}, + {'name': 'publish_tf', 'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'}, + {'name': 'tf_publish_rate', 'default': '0.0', 'description': '[double] rate in Hz for publishing dynamic TF'}, {'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'}, {'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'}, {'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'}, From d7e8792fe9d2de6a8a249b8f6c12205ed9c99b5c Mon Sep 17 00:00:00 2001 From: marqrazz Date: Sun, 23 Apr 2023 12:41:30 -0600 Subject: [PATCH 72/85] update mesh path to use file find --- realsense2_description/urdf/_d415.urdf.xacro | 2 +- realsense2_description/urdf/_d435.urdf.xacro | 2 +- realsense2_description/urdf/_d455.urdf.xacro | 2 +- realsense2_description/urdf/_l515.urdf.xacro | 2 +- realsense2_description/urdf/_r410.urdf.xacro | 2 +- realsense2_description/urdf/_r430.urdf.xacro | 2 +- realsense2_description/urdf/_usb_plug.urdf.xacro | 4 ++-- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/realsense2_description/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro index 282c764cb4..badebebdb4 100644 --- a/realsense2_description/urdf/_d415.urdf.xacro +++ b/realsense2_description/urdf/_d415.urdf.xacro @@ -68,7 +68,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index a8e11faffe..dc0967ff4d 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -75,7 +75,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_d455.urdf.xacro b/realsense2_description/urdf/_d455.urdf.xacro index 509d9720d2..46a9522eec 100644 --- a/realsense2_description/urdf/_d455.urdf.xacro +++ b/realsense2_description/urdf/_d455.urdf.xacro @@ -78,7 +78,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_l515.urdf.xacro b/realsense2_description/urdf/_l515.urdf.xacro index 40d7f58619..6d47a5b76d 100644 --- a/realsense2_description/urdf/_l515.urdf.xacro +++ b/realsense2_description/urdf/_l515.urdf.xacro @@ -87,7 +87,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro index f55028507a..d707bddba4 100644 --- a/realsense2_description/urdf/_r410.urdf.xacro +++ b/realsense2_description/urdf/_r410.urdf.xacro @@ -59,7 +59,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index 0b942abe9a..728d73ce7a 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -60,7 +60,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_usb_plug.urdf.xacro b/realsense2_description/urdf/_usb_plug.urdf.xacro index ba25abcab2..ab1db5d787 100644 --- a/realsense2_description/urdf/_usb_plug.urdf.xacro +++ b/realsense2_description/urdf/_usb_plug.urdf.xacro @@ -36,7 +36,7 @@ aluminum peripherial evaluation case. - + @@ -44,7 +44,7 @@ aluminum peripherial evaluation case. - + From a04ea56d1a0e31c57bc2160f225f84fd5bbe8654 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Sun, 23 Apr 2023 12:47:26 -0600 Subject: [PATCH 73/85] add missing find --- realsense2_description/urdf/_d415.urdf.xacro | 2 +- realsense2_description/urdf/_d435.urdf.xacro | 2 +- realsense2_description/urdf/_d455.urdf.xacro | 2 +- realsense2_description/urdf/_l515.urdf.xacro | 2 +- realsense2_description/urdf/_r410.urdf.xacro | 2 +- realsense2_description/urdf/_r430.urdf.xacro | 2 +- realsense2_description/urdf/_usb_plug.urdf.xacro | 4 ++-- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/realsense2_description/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro index badebebdb4..65266c3160 100644 --- a/realsense2_description/urdf/_d415.urdf.xacro +++ b/realsense2_description/urdf/_d415.urdf.xacro @@ -68,7 +68,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index dc0967ff4d..722c6e3281 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -75,7 +75,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_d455.urdf.xacro b/realsense2_description/urdf/_d455.urdf.xacro index 46a9522eec..8804ab880b 100644 --- a/realsense2_description/urdf/_d455.urdf.xacro +++ b/realsense2_description/urdf/_d455.urdf.xacro @@ -78,7 +78,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_l515.urdf.xacro b/realsense2_description/urdf/_l515.urdf.xacro index 6d47a5b76d..c761ddf734 100644 --- a/realsense2_description/urdf/_l515.urdf.xacro +++ b/realsense2_description/urdf/_l515.urdf.xacro @@ -87,7 +87,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro index d707bddba4..b0121d2b0c 100644 --- a/realsense2_description/urdf/_r410.urdf.xacro +++ b/realsense2_description/urdf/_r410.urdf.xacro @@ -59,7 +59,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index 728d73ce7a..d4ea736bc8 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -60,7 +60,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_usb_plug.urdf.xacro b/realsense2_description/urdf/_usb_plug.urdf.xacro index ab1db5d787..7381756e17 100644 --- a/realsense2_description/urdf/_usb_plug.urdf.xacro +++ b/realsense2_description/urdf/_usb_plug.urdf.xacro @@ -36,7 +36,7 @@ aluminum peripherial evaluation case. - + @@ -44,7 +44,7 @@ aluminum peripherial evaluation case. - + From aa1eab6cbf73e288809c348c06be3bb6c60c7771 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 25 Apr 2023 08:58:10 +0530 Subject: [PATCH 74/85] Fixing TF frame links b/w multi camera nodes when using custom names --- .../launch/rs_multi_camera_launch.py | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/realsense2_camera/launch/rs_multi_camera_launch.py b/realsense2_camera/launch/rs_multi_camera_launch.py index bce56ee1e7..2856067ced 100644 --- a/realsense2_camera/launch/rs_multi_camera_launch.py +++ b/realsense2_camera/launch/rs_multi_camera_launch.py @@ -23,9 +23,9 @@ """Launch realsense2_camera node.""" import copy -from launch import LaunchDescription +from launch import LaunchDescription, LaunchContext import launch_ros.actions -from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, OpaqueFunction from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir from launch.launch_description_sources import PythonLaunchDescriptionSource import sys @@ -47,6 +47,16 @@ def duplicate_params(general_params, posix): param['name'] += posix return local_params +def add_node_action(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') @@ -64,10 +74,5 @@ def generate_launch_description(): PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), launch_arguments=set_configurable_parameters(params2).items(), ), - # dummy static transformation from camera1 to camera2 - launch_ros.actions.Node( - package = "tf2_ros", - executable = "static_transform_publisher", - arguments = ["0", "0", "0", "0", "0", "0", "camera1_link", "camera2_link"] - ), + OpaqueFunction(function=add_node_action) ]) From 82eb970de53919279d3a67587b22e00bd3532e31 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 25 Apr 2023 18:26:56 +0530 Subject: [PATCH 75/85] Moved all TF related functions to tfs.cpp --- realsense2_camera/CMakeLists.txt | 1 + realsense2_camera/src/base_realsense_node.cpp | 311 ----------------- realsense2_camera/src/tfs.cpp | 329 ++++++++++++++++++ 3 files changed, 330 insertions(+), 311 deletions(-) create mode 100644 realsense2_camera/src/tfs.cpp diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index f461a74a7b..78b3002864 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -138,6 +138,7 @@ set(SOURCES src/named_filter.cpp src/profile_manager.cpp src/image_publisher.cpp + src/tfs.cpp ) if(NOT DEFINED ENV{ROS_DISTRO}) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index a427d52d05..310bb25bd5 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -16,7 +16,6 @@ #include "assert.h" #include #include -#include #include #include #include @@ -461,90 +460,6 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) publishMetadata(frame, t, OPTICAL_FRAME_ID(stream_index)); } -void BaseRealSenseNode::pose_callback(rs2::frame frame) -{ - double frame_time = frame.get_timestamp(); - bool placeholder_false(false); - if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) - { - _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); - } - - ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s", - rs2_stream_to_string(frame.get_profile().stream_type()), - frame.get_profile().stream_index(), - rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); - rs2_pose pose = frame.as().get_pose_data(); - rclcpp::Time t(frameSystemTimeSec(frame)); - - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.pose.position.x = -pose.translation.z; - pose_msg.pose.position.y = -pose.translation.x; - pose_msg.pose.position.z = pose.translation.y; - pose_msg.pose.orientation.x = -pose.rotation.z; - pose_msg.pose.orientation.y = -pose.rotation.x; - pose_msg.pose.orientation.z = pose.rotation.y; - pose_msg.pose.orientation.w = pose.rotation.w; - - static tf2_ros::TransformBroadcaster br(_node); - geometry_msgs::msg::TransformStamped msg; - msg.header.stamp = t; - msg.header.frame_id = DEFAULT_ODOM_FRAME_ID; - msg.child_frame_id = FRAME_ID(POSE); - msg.transform.translation.x = pose_msg.pose.position.x; - msg.transform.translation.y = pose_msg.pose.position.y; - msg.transform.translation.z = pose_msg.pose.position.z; - msg.transform.rotation.x = pose_msg.pose.orientation.x; - msg.transform.rotation.y = pose_msg.pose.orientation.y; - msg.transform.rotation.z = pose_msg.pose.orientation.z; - msg.transform.rotation.w = pose_msg.pose.orientation.w; - - if (_publish_odom_tf) br.sendTransform(msg); - - if (0 != _odom_publisher->get_subscription_count()) - { - double cov_pose(_linear_accel_cov * pow(10, 3-(int)pose.tracker_confidence)); - double cov_twist(_angular_velocity_cov * pow(10, 1-(int)pose.tracker_confidence)); - - geometry_msgs::msg::Vector3Stamped v_msg; - tf2::Vector3 tfv(-pose.velocity.z, -pose.velocity.x, pose.velocity.y); - tf2::Quaternion q(-msg.transform.rotation.x,-msg.transform.rotation.y,-msg.transform.rotation.z,msg.transform.rotation.w); - tfv=tf2::quatRotate(q,tfv); - v_msg.vector.x = tfv.x(); - v_msg.vector.y = tfv.y(); - v_msg.vector.z = tfv.z(); - - tfv = tf2::Vector3(-pose.angular_velocity.z, -pose.angular_velocity.x, pose.angular_velocity.y); - tfv=tf2::quatRotate(q,tfv); - geometry_msgs::msg::Vector3Stamped om_msg; - om_msg.vector.x = tfv.x(); - om_msg.vector.y = tfv.y(); - om_msg.vector.z = tfv.z(); - - nav_msgs::msg::Odometry odom_msg; - - odom_msg.header.frame_id = DEFAULT_ODOM_FRAME_ID; - odom_msg.child_frame_id = FRAME_ID(POSE); - odom_msg.header.stamp = t; - odom_msg.pose.pose = pose_msg.pose; - odom_msg.pose.covariance = {cov_pose, 0, 0, 0, 0, 0, - 0, cov_pose, 0, 0, 0, 0, - 0, 0, cov_pose, 0, 0, 0, - 0, 0, 0, cov_twist, 0, 0, - 0, 0, 0, 0, cov_twist, 0, - 0, 0, 0, 0, 0, cov_twist}; - odom_msg.twist.twist.linear = v_msg.vector; - odom_msg.twist.twist.angular = om_msg.vector; - odom_msg.twist.covariance ={cov_pose, 0, 0, 0, 0, 0, - 0, cov_pose, 0, 0, 0, 0, - 0, 0, cov_pose, 0, 0, 0, - 0, 0, 0, cov_twist, 0, 0, - 0, 0, 0, 0, cov_twist, 0, - 0, 0, 0, 0, 0, cov_twist}; - _odom_publisher->publish(odom_msg); - ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); - } -} void BaseRealSenseNode::frame_callback(rs2::frame frame) { @@ -834,145 +749,6 @@ void BaseRealSenseNode::updateExtrinsicsCalibData(const rs2::video_stream_profil _camera_info[right].p.at(7) = -fy * ex.translation[1] + 0.0; // Ty - avoid -0.0 values. } -tf2::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotation[9]) const -{ - Eigen::Matrix3f m; - // We need to be careful about the order, as RS2 rotation matrix is - // column-major, while Eigen::Matrix3f expects row-major. - m << rotation[0], rotation[3], rotation[6], - rotation[1], rotation[4], rotation[7], - rotation[2], rotation[5], rotation[8]; - Eigen::Quaternionf q(m); - return tf2::Quaternion(q.x(), q.y(), q.z(), q.w()); -} - -void BaseRealSenseNode::unpublish_static_tf(const std::string& frame_id, - const std::string& child_frame_id) -{ - std::vector::iterator it; - struct find_tf - { - const std::string& frame_id; - const std::string& child_frame_id; - - // Struct Constructor - find_tf(const std::string& frame_id, const std::string& child_frame_id) : - frame_id(frame_id), child_frame_id(child_frame_id) {} - - bool operator()(const geometry_msgs::msg::TransformStamped &static_tf_msg) const - { - return (static_tf_msg.header.frame_id == frame_id && - static_tf_msg.child_frame_id == child_frame_id); - } - }; - - // Find whether there is any TF with given 'frame_id' and 'child_frame_id' - it = std::find_if(_static_tf_msgs.begin(), _static_tf_msgs.end(), - find_tf(frame_id, child_frame_id)); - - // If found, erase that specific TF - if (it != std::end(_static_tf_msgs)) - { - _static_tf_msgs.erase(it); - } -} - -void BaseRealSenseNode::publish_static_tf(const rclcpp::Time& t, - const float3& trans, - const tf2::Quaternion& q, - const std::string& from, - const std::string& to) -{ - geometry_msgs::msg::TransformStamped msg; - msg.header.stamp = t; - msg.header.frame_id = from; - msg.child_frame_id = to; - - // Convert x,y,z (taken from camera extrinsics) - // from optical cooridnates to ros coordinates - msg.transform.translation.x = trans.z; - msg.transform.translation.y = -trans.x; - msg.transform.translation.z = -trans.y; - - msg.transform.rotation.x = q.getX(); - msg.transform.rotation.y = q.getY(); - msg.transform.rotation.z = q.getZ(); - msg.transform.rotation.w = q.getW(); - _static_tf_msgs.push_back(msg); -} - -void BaseRealSenseNode::publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex){ - // Publish extrinsics topic: - Extrinsics msg = rsExtrinsicsToMsg(ex); - if (_extrinsics_publishers.find(sip) != _extrinsics_publishers.end()) - { - _extrinsics_publishers[sip]->publish(msg); - } -} - -void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile) -{ - // Transform base to stream - stream_index_pair sip(profile.stream_type(), profile.stream_index()); - tf2::Quaternion quaternion_optical; - quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2); - float3 zero_trans{0, 0, 0}; - tf2::Quaternion zero_rot_quaternions; - zero_rot_quaternions.setRPY(0, 0, 0); - - rclcpp::Time transform_ts_ = _node.now(); - - // extrinsic from A to B is the position of A relative to B - // TF from A to B is the transformation to be done on A to get to B - // so, we need to calculate extrinsics in two opposite ways, one for extrinsic topic - // and the second is for transformation topic (TF) - rs2_extrinsics normal_ex; // used to for extrinsics topic - rs2_extrinsics tf_ex; // used for TF - - try - { - normal_ex = base_profile.get_extrinsics_to(profile); - tf_ex = profile.get_extrinsics_to(base_profile); - } - catch (std::exception& e) - { - if (!strcmp(e.what(), "Requested extrinsics are not available!")) - { - ROS_WARN_STREAM("(" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << ") -> (" << rs2_stream_to_string(profile.stream_type()) << ", " << profile.stream_index() << "): " << e.what() << " : using unity as default."); - normal_ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); - tf_ex = normal_ex; - } - else - { - throw e; - } - } - - // publish normal extrinsics e.g. /camera/extrinsics/depth_to_color - publishExtrinsicsTopic(sip, normal_ex); - - // publish static TF - auto Q = rotationMatrixToQuaternion(tf_ex.rotation); - Q = quaternion_optical * Q * quaternion_optical.inverse(); - float3 trans{tf_ex.translation[0], tf_ex.translation[1], tf_ex.translation[2]}; - publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); - - // Transform stream frame to stream optical frame and publish it - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); - - if (profile.is() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1) - { - publish_static_tf(transform_ts_, trans, Q, _base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); - } - - if ((_imu_sync_method > imu_sync_method::NONE) && (profile.stream_type() == RS2_STREAM_GYRO)) - { - publish_static_tf(transform_ts_, zero_trans, zero_rot_quaternions, FRAME_ID(sip), IMU_FRAME_ID); - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); - } -} - void BaseRealSenseNode::SetBaseStream() { const std::vector base_stream_priority = {DEPTH, POSE}; @@ -1003,93 +779,6 @@ void BaseRealSenseNode::SetBaseStream() _base_profile = available_profiles[*base_stream]; } -void BaseRealSenseNode::publishStaticTransforms() -{ - // Since the _static_tf_broadcaster is a latched topic, the old transforms will - // be alive even if the sensors are dynamically disabled. So, reset the - // broadcaster everytime and publish the transforms of enabled sensors alone. - if (_static_tf_broadcaster) - { - _static_tf_broadcaster.reset(); - } - - // intra-process do not support latched QoS, so we need to disable intra-process for this topic - rclcpp::PublisherOptionsWithAllocator> options; - options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - - #ifndef DASHING - _static_tf_broadcaster = std::make_shared(_node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); - #else - _static_tf_broadcaster = std::make_shared(_node, rclcpp::QoS(100), std::move(options)); - #endif - - _static_tf_broadcaster->sendTransform(_static_tf_msgs); -} - -void BaseRealSenseNode::startDynamicTf() -{ - if (!_publish_tf) - { - ROS_WARN("Since the param 'publish_tf' is set to 'false'," - "the value set on the param 'tf_publish_rate' won't have any effect"); - return; - } - if (_tf_publish_rate > 0) - { - ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate); - if (!_tf_t) - { - _tf_t = std::make_shared([this]() - { - publishDynamicTransforms(); - }); - } - } - else - { - if (_tf_t && _tf_t->joinable()) - { - _tf_t->join(); - _tf_t.reset(); - _dynamic_tf_broadcaster.reset(); - ROS_WARN("Stopped publishing dynamic camera transforms (/tf)"); - } - else - { - ROS_WARN("Currently not publishing dynamic camera transforms (/tf)"); - } - } -} - -void BaseRealSenseNode::publishDynamicTransforms() -{ - if (!_dynamic_tf_broadcaster) - { - _dynamic_tf_broadcaster = std::make_shared(_node); - } - - // Publish transforms for the cameras - std::unique_lock lock(_publish_dynamic_tf_mutex); - while (rclcpp::ok() && _is_running && _tf_publish_rate > 0) - { - _cv_tf.wait_for(lock, std::chrono::milliseconds((int)(1000.0/_tf_publish_rate)), [&]{return (!(_is_running && _tf_publish_rate > 0));}); - { - std::lock_guard lock_guard(_publish_tf_mutex); - rclcpp::Time t = _node.now(); - try - { - for(auto& msg : _static_tf_msgs) - msg.header.stamp = t; - _dynamic_tf_broadcaster->sendTransform(_static_tf_msgs); - } - catch(const std::exception& e) - { - ROS_ERROR_STREAM("Error publishing dynamic transforms: " << e.what()); - } - } - } -} - void BaseRealSenseNode::publishPointCloud(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset) { std::string frame_id = (_align_depth_filter->is_enabled() ? OPTICAL_FRAME_ID(COLOR) : OPTICAL_FRAME_ID(DEPTH)); diff --git a/realsense2_camera/src/tfs.cpp b/realsense2_camera/src/tfs.cpp new file mode 100644 index 0000000000..4570fdf96f --- /dev/null +++ b/realsense2_camera/src/tfs.cpp @@ -0,0 +1,329 @@ +// 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. + +#include "../include/base_realsense_node.h" +#include + +using namespace realsense2_camera; + +void BaseRealSenseNode::publish_static_tf(const rclcpp::Time& t, + const float3& trans, + const tf2::Quaternion& q, + const std::string& from, + const std::string& to) +{ + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = t; + msg.header.frame_id = from; + msg.child_frame_id = to; + + // Convert x,y,z (taken from camera extrinsics) + // from optical cooridnates to ros coordinates + msg.transform.translation.x = trans.z; + msg.transform.translation.y = -trans.x; + msg.transform.translation.z = -trans.y; + + msg.transform.rotation.x = q.getX(); + msg.transform.rotation.y = q.getY(); + msg.transform.rotation.z = q.getZ(); + msg.transform.rotation.w = q.getW(); + _static_tf_msgs.push_back(msg); +} + +void BaseRealSenseNode::unpublish_static_tf(const std::string& frame_id, + const std::string& child_frame_id) +{ + std::vector::iterator it; + struct find_tf + { + const std::string& frame_id; + const std::string& child_frame_id; + + // Struct Constructor + find_tf(const std::string& frame_id, const std::string& child_frame_id) : + frame_id(frame_id), child_frame_id(child_frame_id) {} + + bool operator()(const geometry_msgs::msg::TransformStamped &static_tf_msg) const + { + return (static_tf_msg.header.frame_id == frame_id && + static_tf_msg.child_frame_id == child_frame_id); + } + }; + + // Find whether there is any TF with given 'frame_id' and 'child_frame_id' + it = std::find_if(_static_tf_msgs.begin(), _static_tf_msgs.end(), + find_tf(frame_id, child_frame_id)); + + // If found, erase that specific TF + if (it != std::end(_static_tf_msgs)) + { + _static_tf_msgs.erase(it); + } +} + +void BaseRealSenseNode::publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex){ + // Publish extrinsics topic: + Extrinsics msg = rsExtrinsicsToMsg(ex); + if (_extrinsics_publishers.find(sip) != _extrinsics_publishers.end()) + { + _extrinsics_publishers[sip]->publish(msg); + } +} + +tf2::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotation[9]) const +{ + Eigen::Matrix3f m; + // We need to be careful about the order, as RS2 rotation matrix is + // column-major, while Eigen::Matrix3f expects row-major. + m << rotation[0], rotation[3], rotation[6], + rotation[1], rotation[4], rotation[7], + rotation[2], rotation[5], rotation[8]; + Eigen::Quaternionf q(m); + return tf2::Quaternion(q.x(), q.y(), q.z(), q.w()); +} + +void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile) +{ + // Transform base to stream + stream_index_pair sip(profile.stream_type(), profile.stream_index()); + tf2::Quaternion quaternion_optical; + quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2); + float3 zero_trans{0, 0, 0}; + tf2::Quaternion zero_rot_quaternions; + zero_rot_quaternions.setRPY(0, 0, 0); + + rclcpp::Time transform_ts_ = _node.now(); + + // extrinsic from A to B is the position of A relative to B + // TF from A to B is the transformation to be done on A to get to B + // so, we need to calculate extrinsics in two opposite ways, one for extrinsic topic + // and the second is for transformation topic (TF) + rs2_extrinsics normal_ex; // used to for extrinsics topic + rs2_extrinsics tf_ex; // used for TF + + try + { + normal_ex = base_profile.get_extrinsics_to(profile); + tf_ex = profile.get_extrinsics_to(base_profile); + } + catch (std::exception& e) + { + if (!strcmp(e.what(), "Requested extrinsics are not available!")) + { + ROS_WARN_STREAM("(" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << ") -> (" << rs2_stream_to_string(profile.stream_type()) << ", " << profile.stream_index() << "): " << e.what() << " : using unity as default."); + normal_ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); + tf_ex = normal_ex; + } + else + { + throw e; + } + } + + // publish normal extrinsics e.g. /camera/extrinsics/depth_to_color + publishExtrinsicsTopic(sip, normal_ex); + + // publish static TF + auto Q = rotationMatrixToQuaternion(tf_ex.rotation); + Q = quaternion_optical * Q * quaternion_optical.inverse(); + float3 trans{tf_ex.translation[0], tf_ex.translation[1], tf_ex.translation[2]}; + publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); + + // Transform stream frame to stream optical frame and publish it + publish_static_tf(transform_ts_, zero_trans, quaternion_optical, FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); + + if (profile.is() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1) + { + publish_static_tf(transform_ts_, trans, Q, _base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); + publish_static_tf(transform_ts_, zero_trans, quaternion_optical, ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); + } + + if ((_imu_sync_method > imu_sync_method::NONE) && (profile.stream_type() == RS2_STREAM_GYRO)) + { + publish_static_tf(transform_ts_, zero_trans, zero_rot_quaternions, FRAME_ID(sip), IMU_FRAME_ID); + publish_static_tf(transform_ts_, zero_trans, quaternion_optical, IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); + } +} + +void BaseRealSenseNode::publishStaticTransforms() +{ + // Since the _static_tf_broadcaster is a latched topic, the old transforms will + // be alive even if the sensors are dynamically disabled. So, reset the + // broadcaster everytime and publish the transforms of enabled sensors alone. + if (_static_tf_broadcaster) + { + _static_tf_broadcaster.reset(); + } + + // intra-process do not support latched QoS, so we need to disable intra-process for this topic + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + #ifndef DASHING + _static_tf_broadcaster = std::make_shared(_node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); + #else + _static_tf_broadcaster = std::make_shared(_node, rclcpp::QoS(100), std::move(options)); + #endif + + _static_tf_broadcaster->sendTransform(_static_tf_msgs); +} + +void BaseRealSenseNode::startDynamicTf() +{ + if (!_publish_tf) + { + ROS_WARN("Since the param 'publish_tf' is set to 'false'," + "the value set on the param 'tf_publish_rate' won't have any effect"); + return; + } + if (_tf_publish_rate > 0) + { + ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate); + if (!_tf_t) + { + _tf_t = std::make_shared([this]() + { + publishDynamicTransforms(); + }); + } + } + else + { + if (_tf_t && _tf_t->joinable()) + { + _tf_t->join(); + _tf_t.reset(); + _dynamic_tf_broadcaster.reset(); + ROS_WARN("Stopped publishing dynamic camera transforms (/tf)"); + } + else + { + ROS_WARN("Currently not publishing dynamic camera transforms (/tf)"); + } + } +} + +void BaseRealSenseNode::publishDynamicTransforms() +{ + if (!_dynamic_tf_broadcaster) + { + _dynamic_tf_broadcaster = std::make_shared(_node); + } + + // Publish transforms for the cameras + std::unique_lock lock(_publish_dynamic_tf_mutex); + while (rclcpp::ok() && _is_running && _tf_publish_rate > 0) + { + _cv_tf.wait_for(lock, std::chrono::milliseconds((int)(1000.0/_tf_publish_rate)), [&]{return (!(_is_running && _tf_publish_rate > 0));}); + { + std::lock_guard lock_guard(_publish_tf_mutex); + rclcpp::Time t = _node.now(); + try + { + for(auto& msg : _static_tf_msgs) + msg.header.stamp = t; + _dynamic_tf_broadcaster->sendTransform(_static_tf_msgs); + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("Error publishing dynamic transforms: " << e.what()); + } + } + } +} + +void BaseRealSenseNode::pose_callback(rs2::frame frame) +{ + double frame_time = frame.get_timestamp(); + bool placeholder_false(false); + if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) + { + _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain()); + } + + ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s", + rs2_stream_to_string(frame.get_profile().stream_type()), + frame.get_profile().stream_index(), + rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); + rs2_pose pose = frame.as().get_pose_data(); + rclcpp::Time t(frameSystemTimeSec(frame)); + + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.pose.position.x = -pose.translation.z; + pose_msg.pose.position.y = -pose.translation.x; + pose_msg.pose.position.z = pose.translation.y; + pose_msg.pose.orientation.x = -pose.rotation.z; + pose_msg.pose.orientation.y = -pose.rotation.x; + pose_msg.pose.orientation.z = pose.rotation.y; + pose_msg.pose.orientation.w = pose.rotation.w; + + static tf2_ros::TransformBroadcaster br(_node); + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = t; + msg.header.frame_id = DEFAULT_ODOM_FRAME_ID; + msg.child_frame_id = FRAME_ID(POSE); + msg.transform.translation.x = pose_msg.pose.position.x; + msg.transform.translation.y = pose_msg.pose.position.y; + msg.transform.translation.z = pose_msg.pose.position.z; + msg.transform.rotation.x = pose_msg.pose.orientation.x; + msg.transform.rotation.y = pose_msg.pose.orientation.y; + msg.transform.rotation.z = pose_msg.pose.orientation.z; + msg.transform.rotation.w = pose_msg.pose.orientation.w; + + if (_publish_odom_tf) br.sendTransform(msg); + + if (0 != _odom_publisher->get_subscription_count()) + { + double cov_pose(_linear_accel_cov * pow(10, 3-(int)pose.tracker_confidence)); + double cov_twist(_angular_velocity_cov * pow(10, 1-(int)pose.tracker_confidence)); + + geometry_msgs::msg::Vector3Stamped v_msg; + tf2::Vector3 tfv(-pose.velocity.z, -pose.velocity.x, pose.velocity.y); + tf2::Quaternion q(-msg.transform.rotation.x,-msg.transform.rotation.y,-msg.transform.rotation.z,msg.transform.rotation.w); + tfv=tf2::quatRotate(q,tfv); + v_msg.vector.x = tfv.x(); + v_msg.vector.y = tfv.y(); + v_msg.vector.z = tfv.z(); + + tfv = tf2::Vector3(-pose.angular_velocity.z, -pose.angular_velocity.x, pose.angular_velocity.y); + tfv=tf2::quatRotate(q,tfv); + geometry_msgs::msg::Vector3Stamped om_msg; + om_msg.vector.x = tfv.x(); + om_msg.vector.y = tfv.y(); + om_msg.vector.z = tfv.z(); + + nav_msgs::msg::Odometry odom_msg; + + odom_msg.header.frame_id = DEFAULT_ODOM_FRAME_ID; + odom_msg.child_frame_id = FRAME_ID(POSE); + odom_msg.header.stamp = t; + odom_msg.pose.pose = pose_msg.pose; + odom_msg.pose.covariance = {cov_pose, 0, 0, 0, 0, 0, + 0, cov_pose, 0, 0, 0, 0, + 0, 0, cov_pose, 0, 0, 0, + 0, 0, 0, cov_twist, 0, 0, + 0, 0, 0, 0, cov_twist, 0, + 0, 0, 0, 0, 0, cov_twist}; + odom_msg.twist.twist.linear = v_msg.vector; + odom_msg.twist.twist.angular = om_msg.vector; + odom_msg.twist.covariance ={cov_pose, 0, 0, 0, 0, 0, + 0, cov_pose, 0, 0, 0, 0, + 0, 0, cov_pose, 0, 0, 0, + 0, 0, 0, cov_twist, 0, 0, + 0, 0, 0, 0, cov_twist, 0, + 0, 0, 0, 0, 0, cov_twist}; + _odom_publisher->publish(odom_msg); + ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); + } +} From 8fab7a441cb9efc7c4fae4916230afafb877b7bd Mon Sep 17 00:00:00 2001 From: Daniel Honies Date: Tue, 25 Apr 2023 16:44:02 +0200 Subject: [PATCH 76/85] Update realsense_node_factory.cpp Fixes https://github.com/IntelRealSense/realsense-ros/issues/2711 --- realsense2_camera/src/realsense_node_factory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index b129694374..410aacc78f 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -274,7 +274,7 @@ void RealSenseNodeFactory::init() _reconnect_timeout = declare_parameter("reconnect_timeout", 6.0); // A ROS2 hack: until a better way is found to avoid auto convertion of strings containing only digits to integers: - if (_serial_no.front() == '_') _serial_no = _serial_no.substr(1); // remove '_' prefix + if (!_serial_no.empty() && _serial_no.front() == '_') _serial_no = _serial_no.substr(1); // remove '_' prefix std::string rosbag_filename(declare_parameter("rosbag_filename", rclcpp::ParameterValue("")).get()); if (!rosbag_filename.empty()) From 3e87a6c44fccb2582325cc5983cff7884b2c101c Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 27 Apr 2023 00:58:25 +0530 Subject: [PATCH 77/85] Revamped the TF related code --- .../include/base_realsense_node.h | 8 +- realsense2_camera/src/rs_node_setup.cpp | 19 +- realsense2_camera/src/tfs.cpp | 194 +++++++++++------- 3 files changed, 129 insertions(+), 92 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 49ed525d43..cd9aaba667 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -155,18 +155,20 @@ namespace realsense2_camera std::shared_ptr _parameters; std::list _parameters_names; + void restartStaticTransformBroadcaster(); void publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex); - void calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile); + void calcAndAppendTransformMsgs(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile); void getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req, realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res); tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const; - void publish_static_tf(const rclcpp::Time& t, + void append_static_tf_msg(const rclcpp::Time& t, const float3& trans, const tf2::Quaternion& q, const std::string& from, const std::string& to); - void unpublish_static_tf(const std::string& frame_id, + void erase_static_tf_msg(const std::string& frame_id, const std::string& child_frame_id); + void eraseTransformMsgs(const stream_index_pair& sip, const rs2::stream_profile& profile); void setup(); private: diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 36eb734db5..2293856f62 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -198,22 +198,7 @@ void BaseRealSenseNode::stopPublishers(const std::vector& profil if (_publish_tf) { - std::lock_guard lock_guard(_publish_tf_mutex); - - unpublish_static_tf(_base_frame_id, FRAME_ID(sip)); - unpublish_static_tf(FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); - - if (profile.is() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1) - { - unpublish_static_tf(_base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); - unpublish_static_tf(ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); - } - - if ((_imu_sync_method > imu_sync_method::NONE) && (profile.stream_type() == RS2_STREAM_GYRO)) - { - unpublish_static_tf(FRAME_ID(sip), IMU_FRAME_ID); - unpublish_static_tf(IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); - } + eraseTransformMsgs(sip, profile); } } } @@ -355,7 +340,7 @@ void BaseRealSenseNode::updateSensors() std::lock_guard lock_guard(_publish_tf_mutex); for (auto &profile : wanted_profiles) { - calcAndPublishStaticTransform(profile, _base_profile); + calcAndAppendTransformMsgs(profile, _base_profile); } } diff --git a/realsense2_camera/src/tfs.cpp b/realsense2_camera/src/tfs.cpp index 4570fdf96f..4780eb278f 100644 --- a/realsense2_camera/src/tfs.cpp +++ b/realsense2_camera/src/tfs.cpp @@ -17,7 +17,32 @@ using namespace realsense2_camera; -void BaseRealSenseNode::publish_static_tf(const rclcpp::Time& t, +void BaseRealSenseNode::restartStaticTransformBroadcaster() +{ + // Since the _static_tf_broadcaster is a latched topic, the old transforms will + // be alive even if the sensors are dynamically disabled. So, reset the + // broadcaster everytime and publish the transforms of enabled sensors alone. + if (_static_tf_broadcaster) + { + _static_tf_broadcaster.reset(); + } + + // intra-process do not support latched QoS, so we need to disable intra-process for this topic + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + #ifndef DASHING + _static_tf_broadcaster = std::make_shared(_node, + tf2_ros::StaticBroadcasterQoS(), + std::move(options)); + #else + _static_tf_broadcaster = std::make_shared(_node, + rclcpp::QoS(100), + std::move(options)); + #endif +} + +void BaseRealSenseNode::append_static_tf_msg(const rclcpp::Time& t, const float3& trans, const tf2::Quaternion& q, const std::string& from, @@ -41,29 +66,30 @@ void BaseRealSenseNode::publish_static_tf(const rclcpp::Time& t, _static_tf_msgs.push_back(msg); } -void BaseRealSenseNode::unpublish_static_tf(const std::string& frame_id, - const std::string& child_frame_id) +struct compare_tf_ids { - std::vector::iterator it; - struct find_tf - { - const std::string& frame_id; - const std::string& child_frame_id; + const std::string& frame_id; + const std::string& child_frame_id; - // Struct Constructor - find_tf(const std::string& frame_id, const std::string& child_frame_id) : - frame_id(frame_id), child_frame_id(child_frame_id) {} + // Struct Constructor + compare_tf_ids(const std::string& frame_id, const std::string& child_frame_id) : + frame_id(frame_id), child_frame_id(child_frame_id) {} - bool operator()(const geometry_msgs::msg::TransformStamped &static_tf_msg) const - { - return (static_tf_msg.header.frame_id == frame_id && - static_tf_msg.child_frame_id == child_frame_id); - } - }; + bool operator()(const geometry_msgs::msg::TransformStamped &static_tf_msg) const + { + return (static_tf_msg.header.frame_id == frame_id && + static_tf_msg.child_frame_id == child_frame_id); + } +}; + +void BaseRealSenseNode::erase_static_tf_msg(const std::string& frame_id, + const std::string& child_frame_id) +{ + std::vector::iterator it; // Find whether there is any TF with given 'frame_id' and 'child_frame_id' it = std::find_if(_static_tf_msgs.begin(), _static_tf_msgs.end(), - find_tf(frame_id, child_frame_id)); + compare_tf_ids(frame_id, child_frame_id)); // If found, erase that specific TF if (it != std::end(_static_tf_msgs)) @@ -93,7 +119,8 @@ tf2::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotati return tf2::Quaternion(q.x(), q.y(), q.z(), q.w()); } -void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile) +void BaseRealSenseNode::calcAndAppendTransformMsgs(const rs2::stream_profile& profile, + const rs2::stream_profile& base_profile) { // Transform base to stream stream_index_pair sip(profile.stream_type(), profile.stream_index()); @@ -121,7 +148,12 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& { if (!strcmp(e.what(), "Requested extrinsics are not available!")) { - ROS_WARN_STREAM("(" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << ") -> (" << rs2_stream_to_string(profile.stream_type()) << ", " << profile.stream_index() << "): " << e.what() << " : using unity as default."); + ROS_WARN_STREAM("(" << rs2_stream_to_string(base_profile.stream_type()) << ", " + << base_profile.stream_index() << ") -> (" + << rs2_stream_to_string(profile.stream_type()) << ", " + << profile.stream_index() << "): " + << e.what() + << " : using unity as default."); normal_ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); tf_ex = normal_ex; } @@ -138,47 +170,87 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile& auto Q = rotationMatrixToQuaternion(tf_ex.rotation); Q = quaternion_optical * Q * quaternion_optical.inverse(); float3 trans{tf_ex.translation[0], tf_ex.translation[1], tf_ex.translation[2]}; - publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); + + append_static_tf_msg(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); // Transform stream frame to stream optical frame and publish it - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); + append_static_tf_msg(transform_ts_, zero_trans, quaternion_optical, FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); - if (profile.is() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1) + if (profile.is() && + profile.stream_type() != RS2_STREAM_DEPTH && + profile.stream_index() == 1) { - publish_static_tf(transform_ts_, trans, Q, _base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); + append_static_tf_msg(transform_ts_, trans, Q, _base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); + append_static_tf_msg(transform_ts_, zero_trans, quaternion_optical, + ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); } if ((_imu_sync_method > imu_sync_method::NONE) && (profile.stream_type() == RS2_STREAM_GYRO)) { - publish_static_tf(transform_ts_, zero_trans, zero_rot_quaternions, FRAME_ID(sip), IMU_FRAME_ID); - publish_static_tf(transform_ts_, zero_trans, quaternion_optical, IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); + append_static_tf_msg(transform_ts_, zero_trans, zero_rot_quaternions, FRAME_ID(sip), IMU_FRAME_ID); + append_static_tf_msg(transform_ts_, zero_trans, quaternion_optical, IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); } } -void BaseRealSenseNode::publishStaticTransforms() +void BaseRealSenseNode::eraseTransformMsgs(const stream_index_pair& sip, const rs2::stream_profile& profile) { - // Since the _static_tf_broadcaster is a latched topic, the old transforms will - // be alive even if the sensors are dynamically disabled. So, reset the - // broadcaster everytime and publish the transforms of enabled sensors alone. - if (_static_tf_broadcaster) + std::lock_guard lock_guard(_publish_tf_mutex); + + erase_static_tf_msg(_base_frame_id, FRAME_ID(sip)); + erase_static_tf_msg(FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); + + if (profile.is() && + profile.stream_type() != RS2_STREAM_DEPTH && + profile.stream_index() == 1) { - _static_tf_broadcaster.reset(); + erase_static_tf_msg(_base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip)); + erase_static_tf_msg(ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); } - // intra-process do not support latched QoS, so we need to disable intra-process for this topic - rclcpp::PublisherOptionsWithAllocator> options; - options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + if ((_imu_sync_method > imu_sync_method::NONE) && (profile.stream_type() == RS2_STREAM_GYRO)) + { + erase_static_tf_msg(FRAME_ID(sip), IMU_FRAME_ID); + erase_static_tf_msg(IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID); + } +} - #ifndef DASHING - _static_tf_broadcaster = std::make_shared(_node, tf2_ros::StaticBroadcasterQoS(), std::move(options)); - #else - _static_tf_broadcaster = std::make_shared(_node, rclcpp::QoS(100), std::move(options)); - #endif +void BaseRealSenseNode::publishStaticTransforms() +{ + restartStaticTransformBroadcaster(); _static_tf_broadcaster->sendTransform(_static_tf_msgs); } +void BaseRealSenseNode::publishDynamicTransforms() +{ + if (!_dynamic_tf_broadcaster) + { + _dynamic_tf_broadcaster = std::make_shared(_node); + } + + // Publish transforms for the cameras + std::unique_lock lock(_publish_dynamic_tf_mutex); + while (rclcpp::ok() && _is_running && _tf_publish_rate > 0) + { + _cv_tf.wait_for(lock, std::chrono::milliseconds((int)(1000.0/_tf_publish_rate)), + [&]{return (!(_is_running && _tf_publish_rate > 0));}); + { + std::lock_guard lock_guard(_publish_tf_mutex); + rclcpp::Time t = _node.now(); + try + { + for(auto& msg : _static_tf_msgs) + msg.header.stamp = t; + _dynamic_tf_broadcaster->sendTransform(_static_tf_msgs); + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("Error publishing dynamic transforms: " << e.what()); + } + } + } +} + void BaseRealSenseNode::startDynamicTf() { if (!_publish_tf) @@ -189,6 +261,7 @@ void BaseRealSenseNode::startDynamicTf() } if (_tf_publish_rate > 0) { + // Start publishing dynamic TF, if the param 'tf_publish_rate' is set to > 0.0 Hz ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate); if (!_tf_t) { @@ -202,6 +275,7 @@ void BaseRealSenseNode::startDynamicTf() { if (_tf_t && _tf_t->joinable()) { + // Stop publishing dynamic TF by resetting the '_tf_t' thread and '_dynamic_tf_broadcaster' _tf_t->join(); _tf_t.reset(); _dynamic_tf_broadcaster.reset(); @@ -209,36 +283,9 @@ void BaseRealSenseNode::startDynamicTf() } else { - ROS_WARN("Currently not publishing dynamic camera transforms (/tf)"); - } - } -} - -void BaseRealSenseNode::publishDynamicTransforms() -{ - if (!_dynamic_tf_broadcaster) - { - _dynamic_tf_broadcaster = std::make_shared(_node); - } - - // Publish transforms for the cameras - std::unique_lock lock(_publish_dynamic_tf_mutex); - while (rclcpp::ok() && _is_running && _tf_publish_rate > 0) - { - _cv_tf.wait_for(lock, std::chrono::milliseconds((int)(1000.0/_tf_publish_rate)), [&]{return (!(_is_running && _tf_publish_rate > 0));}); - { - std::lock_guard lock_guard(_publish_tf_mutex); - rclcpp::Time t = _node.now(); - try - { - for(auto& msg : _static_tf_msgs) - msg.header.stamp = t; - _dynamic_tf_broadcaster->sendTransform(_static_tf_msgs); - } - catch(const std::exception& e) - { - ROS_ERROR_STREAM("Error publishing dynamic transforms: " << e.what()); - } + // '_tf_t' thread is not running currently. i.e, dynamic tf is not getting broadcasted. + ROS_WARN("Currently not publishing dynamic camera transforms (/tf). " + "To start publishing it, set the 'tf_publish_rate' param to > 0.0 Hz "); } } } @@ -290,7 +337,10 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame) geometry_msgs::msg::Vector3Stamped v_msg; tf2::Vector3 tfv(-pose.velocity.z, -pose.velocity.x, pose.velocity.y); - tf2::Quaternion q(-msg.transform.rotation.x,-msg.transform.rotation.y,-msg.transform.rotation.z,msg.transform.rotation.w); + tf2::Quaternion q(-msg.transform.rotation.x, + -msg.transform.rotation.y, + -msg.transform.rotation.z, + msg.transform.rotation.w); tfv=tf2::quatRotate(q,tfv); v_msg.vector.x = tfv.x(); v_msg.vector.y = tfv.y(); From f4c7dd88f6534ad922c29467817ac28f380310f0 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 13 Jun 2023 05:28:05 +0530 Subject: [PATCH 78/85] Applying AlignDepth filter after Pointcloud --- realsense2_camera/src/base_realsense_node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 310bb25bd5..bf44d360b2 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -195,14 +195,15 @@ void BaseRealSenseNode::setupFilters() } _cv_mpc.notify_one(); }; - _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); - _filters.push_back(_align_depth_filter); _colorizer_filter = std::make_shared(std::make_shared(), _parameters, _logger); _filters.push_back(_colorizer_filter); _pc_filter = std::make_shared(std::make_shared(), _node, _parameters, _logger); _filters.push_back(_pc_filter); + + _align_depth_filter = std::make_shared(std::make_shared(RS2_STREAM_COLOR), update_align_depth_func, _parameters, _logger); + _filters.push_back(_align_depth_filter); } cv::Mat& BaseRealSenseNode::fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image) From 0064ae8fb4f07649bb094a3082923a6b00e535c4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 14 Jun 2023 10:36:56 +0200 Subject: [PATCH 79/85] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 06ba97918f..7f9e18a6d3 100644 --- a/README.md +++ b/README.md @@ -466,7 +466,7 @@ Each of the above filters have it's own parameters, following the naming convent Efficient intra-process communication:

-Our ROS2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can reduce copy times on image topics (not point-cloud or others), especially with big frame resolutions and high FPS. +Our ROS2 Wrapper node supports zero-copy communications if loaded in the same process as a subscriber node. This can reduce copy times on image/pointcloud topics, especially with big frame resolutions and high FPS. You will need to launch a component container and launch our node as a component together with other component nodes. Further details on "Composing multiple nodes in a single process" can be found [here](https://docs.ros.org/en/rolling/Tutorials/Composition.html). From 6c866cc783f5d0cdd7f9da3f744f60225785bb9a Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Wed, 21 Jun 2023 22:27:18 +0300 Subject: [PATCH 80/85] Update main.yml --- .github/workflows/main.yml | 35 ++++------------------------------- 1 file changed, 4 insertions(+), 31 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 58d5ebe7f4..d6bad8a800 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -29,10 +29,8 @@ jobs: os: ubuntu-22.04 - ros_distro: 'humble' os: ubuntu-22.04 - - ros_distro: 'galactic' - os: ubuntu-20.04 - - ros_distro: 'foxy' - os: ubuntu-20.04 + - ros_distro: 'iron' + os: ubuntu-22.04 steps: @@ -59,37 +57,12 @@ jobs: with: cmake-version: '3.23.0' - ## If distro is one of [galactic, foxy], use the setup-ros@v0.2 - ## which supports old versions of ROS2 and EOL versions. - ## For Humble distro, use v0.3 - ## See: 1) https://github.com/ros-tooling/setup-ros#Supported-platforms - ## 2) https://github.com/ros-tooling/setup-ros/tree/v0.2#Supported-platforms - - name: build ROS2 Galactic/Foxy - if: ${{ matrix.ros_distro != 'humble' && matrix.ros_distro != 'rolling'}} - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: ${{ matrix.ros_distro }} - - - name: build ROS2 Humble - if: ${{ matrix.ros_distro == 'humble' || matrix.ros_distro == 'rolling' }} + - name: build ROS2 uses: ros-tooling/setup-ros@v0.3 with: required-ros-distributions: ${{ matrix.ros_distro }} - - ## For all distros except humble and rolling, install RealSense SDK From Debians - ## For humble + rolling distros, install from source (TODO: Change this when we have public librealsense2 debians for Ubuntu22) - - name: Install RealSense SDK 2.0 Dependencies - if: ${{ matrix.ros_distro != 'humble' && matrix.ros_distro != 'rolling' }} - run: | - sudo apt-get update - sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com --recv-key C8B3A55A6F3EFCDE - sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" - sudo apt-get update -qq - sudo apt-get install librealsense2-dev --allow-unauthenticated -y - sudo apt-get update - + - name: Build RealSense SDK 2.0 from source - if: ${{ matrix.ros_distro == 'humble' || matrix.ros_distro == 'rolling' }} run: | cd ${{github.workspace}} git clone https://github.com/IntelRealSense/librealsense.git -b master From 0e894443adf92107119a25065925ca62ea0810ea Mon Sep 17 00:00:00 2001 From: Samer Khshiboun <99127997+SamerKhshiboun@users.noreply.github.com> Date: Wed, 21 Jun 2023 22:27:43 +0300 Subject: [PATCH 81/85] Update pre-release.yml --- .github/workflows/pre-release.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 3228fa6c6c..62e43c9477 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -31,12 +31,12 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [humble, foxy] + ros_distro: [humble, iron] include: - ros_distro: 'humble' os: ubuntu-22.04 - - ros_distro: 'foxy' - os: ubuntu-20.04 + - ros_distro: 'iron' + os: ubuntu-22.04 env: ROS_DISTRO: ${{ matrix.ros_distro }} From 23f616d12e6ca458347c9cac654fff28d582d32e Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Wed, 21 Jun 2023 22:43:19 +0300 Subject: [PATCH 82/85] add iron to CMakeList.txt update readme --- .github/workflows/main.yml | 21 ++++++--------------- .github/workflows/pre-release.yml | 6 +++--- README.md | 17 ++++++----------- realsense2_camera/CMakeLists.txt | 4 ++++ 4 files changed, 19 insertions(+), 29 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index d6bad8a800..a07f25fba2 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -23,14 +23,14 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [rolling, humble, galactic, foxy] + ros_distro: [rolling, iron, humble] include: - ros_distro: 'rolling' os: ubuntu-22.04 - - ros_distro: 'humble' - os: ubuntu-22.04 - ros_distro: 'iron' os: ubuntu-22.04 + - ros_distro: 'humble' + os: ubuntu-22.04 steps: @@ -48,24 +48,15 @@ jobs: cd ${{github.workspace}}/ros2/src/realsense-ros/scripts ./pr_check.sh - # Workaround - forcing cmake 3.23.0 instead of using latest cmake of GHA. - # cmake 3.24.0 is not compaitable with galactic/foxy/humble ament_cmake - # see: https://github.com/ament/ament_cmake/pull/395 - # TODO: remove this step once ament_cmake for galactic/foxy/humble is released - - name: Setup cmake 3.23.0 - uses: jwlawson/actions-setup-cmake@v1.12.1 - with: - cmake-version: '3.23.0' - - name: build ROS2 - uses: ros-tooling/setup-ros@v0.3 + uses: ros-tooling/setup-ros@v0.6 with: required-ros-distributions: ${{ matrix.ros_distro }} - name: Build RealSense SDK 2.0 from source run: | cd ${{github.workspace}} - git clone https://github.com/IntelRealSense/librealsense.git -b master + git clone https://github.com/IntelRealSense/librealsense.git -b development cd librealsense sudo mkdir build cd build @@ -75,7 +66,7 @@ jobs: sudo make -j10 sudo make install - - name: Build + - name: Build RealSense ROS2 Wrapper run: | echo "source /opt/ros/${{ matrix.ros_distro }}/setup.bash" >> ${{github.workspace}}/.bashrc source ${{github.workspace}}/.bashrc diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 62e43c9477..4b54702bcc 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -31,12 +31,12 @@ jobs: strategy: fail-fast: false matrix: - ros_distro: [humble, iron] + ros_distro: [iron, humble] include: - - ros_distro: 'humble' - os: ubuntu-22.04 - ros_distro: 'iron' os: ubuntu-22.04 + - ros_distro: 'humble' + os: ubuntu-22.04 env: ROS_DISTRO: ${{ matrix.ros_distro }} diff --git a/README.md b/README.md index 06ba97918f..ad3f134f71 100644 --- a/README.md +++ b/README.md @@ -11,9 +11,8 @@ [![rolling][rolling-badge]][rolling] +[![iron][iron-badge]][iron] [![humble][humble-badge]][humble] -[![foxy][foxy-badge]][foxy] -[![galactic][galactic-badge]][galactic] [![ubuntu22][ubuntu22-badge]][ubuntu22] [![ubuntu20][ubuntu20-badge]][ubuntu20] @@ -89,10 +88,8 @@ - #### Ubuntu 22.04: + - [ROS2 Iron](https://docs.ros.org/en/iron/Installation/Ubuntu-Install-Debians.html) - [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) -- #### Ubuntu 20.04: - - [ROS2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) - - [ROS2 Galactic](https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html) (deprecated) @@ -156,7 +153,7 @@ - Source environment ```bash - ROS_DISTRO= # set your ROS_DISTRO: humble, galactic, foxy + ROS_DISTRO= # set your ROS_DISTRO: iron, humble source /opt/ros/$ROS_DISTRO/setup.bash cd ~/ros2_ws . install/local_setup.bash @@ -470,7 +467,7 @@ Our ROS2 Wrapper node supports zero-copy communications if loaded in the same pr You will need to launch a component container and launch our node as a component together with other component nodes. Further details on "Composing multiple nodes in a single process" can be found [here](https://docs.ros.org/en/rolling/Tutorials/Composition.html). -Further details on efficient intra-process communication can be found [here](https://docs.ros.org/en/foxy/Tutorials/Intra-Process-Communication.html#efficient-intra-process-communication). +Further details on efficient intra-process communication can be found [here](https://docs.ros.org/en/humble/Tutorials/Intra-Process-Communication.html#efficient-intra-process-communication). ### Example #### Manually loading multiple components into the same process @@ -513,10 +510,8 @@ ros2 launch realsense2_camera rs_intra_process_demo_launch.py intra_process_comm [rolling]: https://docs.ros.org/en/rolling/index.html [humble-badge]: https://img.shields.io/badge/-HUMBLE-orange?style=flat-square&logo=ros [humble]: https://docs.ros.org/en/humble/index.html -[foxy-badge]: https://img.shields.io/badge/-FOXY-orange?style=flat-square&logo=ros -[foxy]: https://docs.ros.org/en/foxy/index.html -[galactic-badge]: https://img.shields.io/badge/-GALACTIC-orange?style=flat-square&logo=ros -[galactic]: https://docs.ros.org/en/galactic/index.html +[iron-badge]: https://img.shields.io/badge/-IRON-orange?style=flat-square&logo=ros +[iron]: https://docs.ros.org/en/iron/index.html [ubuntu22-badge]: https://img.shields.io/badge/-UBUNTU%2022%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white [ubuntu22]: https://releases.ubuntu.com/jammy/ [ubuntu20-badge]: https://img.shields.io/badge/-UBUNTU%2020%2E04-blue?style=flat-square&logo=ubuntu&logoColor=white diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 78b3002864..fb55b1a9f3 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -164,6 +164,10 @@ elseif("$ENV{ROS_DISTRO}" STREQUAL "humble") message(STATUS "Build for ROS2 Humble") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHUMBLE") set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) +elseif("$ENV{ROS_DISTRO}" STREQUAL "iron") + message(STATUS "Build for ROS2 Iron") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DIRON") + set(SOURCES "${SOURCES}" src/ros_param_backend_foxy.cpp) elseif("$ENV{ROS_DISTRO}" STREQUAL "rolling") message(STATUS "Build for ROS2 Rolling") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DROLLING") From d617638a695f44c7778af02fcc9385105ef38b64 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Tue, 27 Jun 2023 22:56:30 +0530 Subject: [PATCH 83/85] Publish /aligned_depth_to_color topic only when color frame present --- realsense2_camera/src/base_realsense_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 310bb25bd5..9892f15a0f 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -499,6 +499,8 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) clip_depth(original_depth_frame, _clipping_distance); } + rs2::video_frame original_color_frame = frameset.get_color_frame(); + ROS_DEBUG("num_filters: %d", static_cast(_filters.size())); for (auto filter_it : _filters) { @@ -529,7 +531,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) { if (sent_depth_frame) continue; sent_depth_frame = true; - if (_align_depth_filter->is_enabled()) + if (original_color_frame && _align_depth_filter->is_enabled()) { publishFrame(f, t, COLOR, _depth_aligned_image, From 8fce83cb9a7686cf1e1c00ee7aa4c2f9dcc12e87 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 27 Jun 2023 15:13:32 +0300 Subject: [PATCH 84/85] bump version to 4.54.1 --- realsense2_camera/CHANGELOG.rst | 65 +++++++++++++++++++++++++++ realsense2_camera/CMakeLists.txt | 2 +- realsense2_camera/include/constants.h | 2 +- realsense2_camera_msgs/CHANGELOG.rst | 8 ++++ realsense2_description/CHANGELOG.rst | 9 ++++ 5 files changed, 84 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst index d4dc9029c5..4edb915e23 100644 --- a/realsense2_camera/CHANGELOG.rst +++ b/realsense2_camera/CHANGELOG.rst @@ -2,6 +2,71 @@ Changelog for package realsense2_camera ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Applying AlignDepth filter after Pointcloud +* Publish /aligned_depth_to_color topic only when color frame present +* Support Iron distro +* Protect empty string dereference +* Fix: /tf and /static_tf topics' inconsistencies +* Revamped the TF related code +* Fixing TF frame links b/w multi camera nodes when using custom names +* Updated TF descriptions in launch py and readme +* Fixing /tf topic has only TFs of last started sensor +* add D430i support +* Fix Swapped TFs Axes +* replace stereo module with depth module +* use rs2_to_ros to replace stereo module with depth moudle +* calculate extriniscs twice in two opposite ways to save inverting rotation matrix +* fix matrix rotation +* Merge branch 'ros2-development' into readme_fix +* invert translation +* Added 'publish_tf' param in rs launch files +* Indentation corrections +* Fix: Don't publish /tf when publish_tf is false +* use playback device for rosbags +* Avoid configuring dynamic_tf_broadcaster within tf_publish_rate param's callback +* Fix lower FPS in D405, D455 +* update rs_launch.py to support enable_auto_exposure and manual exposure +* fix timestamp calculation metadata header to be aligned with metadata json timestamp +* Expose USB port in DeviceInfo service +* Use latched QoS for Extrinsic topic when intra-process is used +* add cppcheck to GHA +* Fix Apache License Header and Intel Copyrights +* apply copyrights and license on project +* Enable intra-process communication for point clouds +* Fix ros2 parameter descriptions and range values +* T265 clean up +* fix float_to_double method +* realsense2_camera/src/sensor_params.cpp +* remove T265 device from ROS Wrapper - step1 +* Enable D457 +* Fix hdr_merge filter initialization in ros2 launch +* if default profile is not defined, take the first available profile as default +* changed to static_cast and added descriptor name and type +* remove extra ';' +* remove unused variable format_str +* publish point cloud via unique shared pointer +* make source backward compatible to older versions of cv_bridge and rclcpp +* add hdr_merge.enable and depth_module.hdr_enabled to rs_launch.py +* fix compilation errors +* fix tabs +* if default profile is not defined, take the first available profile as default +* Fix ros2 sensor controls steps and add control default value to param description +* Publish static transforms when intra porocess communication is enabled +* Properly read camera config files in rs_launch.py +* fix deprecated API +* Add D457 +* Windows bring-up +* publish actual IMU optical frame ID in IMU messages +* Publish static tf for IMU frames +* fix extrinsics calculation +* fix ordered_pc arg prefix +* publish IMU frames only if unite/sync imu method is not none +* Publish static tf for IMU frames +* add D430i support +* Contributors: Arun Prasad, Arun Prasad V, Arun-Prasad-V, Christian Rauch, Daniel Honies, Gilad Bretter, Nir Azkiel, NirAz, Pranav Dhulipala, Samer Khshiboun, SamerKhshiboun, Stephan Wirth, Xiangyu, Yadunund, nvidia + 4.51.1 (2022-09-13) ------------------- * Fix crash when activating IMU & aligned depth together diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index fb55b1a9f3..4b26fdab08 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -116,7 +116,7 @@ find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(diagnostic_updater REQUIRED) -find_package(realsense2 2.51.1) +find_package(realsense2 2.54.1) if(NOT realsense2_FOUND) message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") endif() diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index f451222a74..e5ec5a2413 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -18,7 +18,7 @@ #include #define REALSENSE_ROS_MAJOR_VERSION 4 -#define REALSENSE_ROS_MINOR_VERSION 51 +#define REALSENSE_ROS_MINOR_VERSION 54 #define REALSENSE_ROS_PATCH_VERSION 1 #define STRINGIFY(arg) #arg diff --git a/realsense2_camera_msgs/CHANGELOG.rst b/realsense2_camera_msgs/CHANGELOG.rst index 51029bbe73..2df89c45c7 100644 --- a/realsense2_camera_msgs/CHANGELOG.rst +++ b/realsense2_camera_msgs/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package realsense2_camera_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* add info about extrinsic msg format in Extrinsics.msg and README.md +* Expose USB port in DeviceInfo service +* Fix Apache License Header and Intel Copyrights +* apply copyrights and license on project +* Contributors: Arun Prasad, Nir Azkiel, SamerKhshiboun, Stephan Wirth + 4.51.1 (2022-09-13) ------------------- * Add copyright and license to all ROS2-beta source files diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst index 249b0e3b44..7140732886 100644 --- a/realsense2_description/CHANGELOG.rst +++ b/realsense2_description/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package realsense2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update mesh path +* clone PR1637 to ros2-development +* Fix Apache License Header and Intel Copyrights +* apply copyrights and license on project +* Replace deprecated parameter node_name with name +* Contributors: Arun Prasad, Nir Azkiel, SamerKhshiboun, augustelalande, marqrazz + 4.51.1 (2022-09-13) ------------------- * Add copyright and license to all ROS2-beta source files From ae45d4969f5a7105a6a9e0deeadc785004d72441 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Tue, 27 Jun 2023 15:14:57 +0300 Subject: [PATCH 85/85] 4.54.1 --- realsense2_camera/CHANGELOG.rst | 4 ++-- realsense2_camera/package.xml | 2 +- realsense2_camera_msgs/CHANGELOG.rst | 4 ++-- realsense2_camera_msgs/package.xml | 2 +- realsense2_description/CHANGELOG.rst | 4 ++-- realsense2_description/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst index 4edb915e23..a1923a8e60 100644 --- a/realsense2_camera/CHANGELOG.rst +++ b/realsense2_camera/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_camera ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.54.1 (2023-06-27) +------------------- * Applying AlignDepth filter after Pointcloud * Publish /aligned_depth_to_color topic only when color frame present * Support Iron distro diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 0556f105ba..9a46a9e51e 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -2,7 +2,7 @@ realsense2_camera - 4.51.1 + 4.54.1 RealSense camera package allowing access to Intel SR300 and D400 3D cameras LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_camera_msgs/CHANGELOG.rst b/realsense2_camera_msgs/CHANGELOG.rst index 2df89c45c7..5f1c979598 100644 --- a/realsense2_camera_msgs/CHANGELOG.rst +++ b/realsense2_camera_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_camera_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.54.1 (2023-06-27) +------------------- * add info about extrinsic msg format in Extrinsics.msg and README.md * Expose USB port in DeviceInfo service * Fix Apache License Header and Intel Copyrights diff --git a/realsense2_camera_msgs/package.xml b/realsense2_camera_msgs/package.xml index b87821fda3..8c527be1fe 100644 --- a/realsense2_camera_msgs/package.xml +++ b/realsense2_camera_msgs/package.xml @@ -2,7 +2,7 @@ realsense2_camera_msgs - 4.51.1 + 4.54.1 RealSense camera_msgs package containing realsense camera messages definitions LibRealSense ROS Team Apache License 2.0 diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst index 7140732886..6580fcf252 100644 --- a/realsense2_description/CHANGELOG.rst +++ b/realsense2_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.54.1 (2023-06-27) +------------------- * Update mesh path * clone PR1637 to ros2-development * Fix Apache License Header and Intel Copyrights diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 0a7776b968..bc9f573032 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -2,7 +2,7 @@ realsense2_description - 4.51.1 + 4.54.1 RealSense description package for Intel 3D D400 cameras LibRealSense ROS Team Apache License 2.0