PR #3293 from remibettan: align_depth_to_infra2 enabled, pointcloud a…

…nd align_depth filters to own files

realsense2_camera/CMakeLists.txt

@@ -144,6 +144,8 @@ set(SOURCES
     src/dynamic_params.cpp
     src/sensor_params.cpp
     src/named_filter.cpp
+    src/pointcloud_filter.cpp
+    src/align_depth_filter.cpp
     src/profile_manager.cpp
     src/image_publisher.cpp
     src/tfs.cpp
@@ -205,6 +207,8 @@ set(INCLUDES
     include/dynamic_params.h
     include/sensor_params.h
     include/named_filter.h
+    include/pointcloud_filter.h
+    include/align_depth_filter.h
     include/ros_param_backend.h
     include/profile_manager.h
     include/image_publisher.h)

realsense2_camera/examples/pointcloud/rs_pointcloud_infra_launch.py

@@ -0,0 +1,58 @@
+# Copyright 2025 Intel Corporation. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# DESCRIPTION #
+# ----------- #
+# Use this launch file to launch a device and align depth to infrared 2.
+# The Parameters available for definition in the command line for the camera are described in rs_launch.configurable_parameters
+# command line example:
+# ros2 launch realsense2_camera rs_align_depth_to infra_launch.py
+
+"""Launch realsense2_camera node."""
+from launch import LaunchDescription
+import launch_ros.actions
+from launch.actions import OpaqueFunction
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+import sys
+import pathlib
+sys.path.append(str(pathlib.Path(__file__).parent.absolute()))
+import os
+from ament_index_python.packages import get_package_share_directory
+sys.path.append(os.path.join(get_package_share_directory('realsense2_camera'), 'launch'))
+import rs_launch
+
+local_parameters = [{'name': 'camera_name',                  'default': 'camera', 'description': 'camera unique name'},
+                    {'name': 'camera_namespace',             'default': 'camera', 'description': 'camera namespace'},
+                    {'name': 'enable_color',                 'default': 'false', 'description': 'enable color stream'},
+                    {'name': 'enable_depth',                 'default': 'true', 'description': 'enable depth stream'},
+                    {'name': 'enable_infra2',                'default': 'true', 'description': 'enable depth stream'},
+                    {'name': 'enable_sync',                  'default': 'true', 'description': 'enable sync mode'},
+                    {'name': 'pointcloud.enable',            'default': 'true', 'description': 'enable pointcloud'},
+                    {'name': 'pointcloud.stream_filter',     'default': '3', 'description': 'pointcloud with infrared'},
+                   ]
+
+def set_configurable_parameters(local_params):
+    return dict([(param['name'], LaunchConfiguration(param['name'])) for param in local_params])
+
+
+def generate_launch_description():
+    params = rs_launch.configurable_parameters
+    return LaunchDescription(
+        rs_launch.declare_configurable_parameters(local_parameters) +
+        rs_launch.declare_configurable_parameters(params) + 
+        [
+        OpaqueFunction(function=rs_launch.launch_setup,
+                kwargs = {'params' : set_configurable_parameters(params)}
+        )
+    ])

realsense2_camera/include/align_depth_filter.h

@@ -0,0 +1,33 @@
+// Copyright 2025 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 <string>
+#include <memory>
+#include <librealsense2/rs.hpp>
+#include <sensor_params.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <ros_sensor.h>
+#include "named_filter.h"
+
+namespace realsense2_camera
+{
+    class AlignDepthFilter : public NamedFilter
+    {
+        public:
+            AlignDepthFilter(std::shared_ptr<rs2::filter> filter, std::function<void(const rclcpp::Parameter&)> update_align_depth_func,
+                std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled = false);
+    };
+}

realsense2_camera/include/base_realsense_node.h

@@ -112,6 +112,8 @@ namespace realsense2_camera
             bool                                                _is_enabled;
     };
 
+    class AlignDepthFilter;
+    class PointcloudFilter;
     class BaseRealSenseNode
     {
     public:

realsense2_camera/include/named_filter.h

@@ -47,32 +47,4 @@ namespace realsense2_camera
             rclcpp::Logger _logger;
 
     };
-
-    class PointcloudFilter : public NamedFilter
-    {
-        public:
-            PointcloudFilter(std::shared_ptr<rs2::filter> filter, rclcpp::Node& node, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled=false);
-
-            void setPublisher();
-            void Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id);
-
-        private:
-            void setParameters();
-
-        private:
-            bool _is_enabled_pc;
-            rclcpp::Node& _node;
-            bool _allow_no_texture_points;
-            bool _ordered_pc;
-            std::mutex _mutex_publisher;
-            rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _pointcloud_publisher;
-            std::string _pointcloud_qos;
-    };
-
-    class AlignDepthFilter : public NamedFilter
-    {
-        public:
-            AlignDepthFilter(std::shared_ptr<rs2::filter> filter, std::function<void(const rclcpp::Parameter&)> update_align_depth_func,
-                std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled = false);
-    };
 }

realsense2_camera/include/pointcloud_filter.h

@@ -0,0 +1,47 @@
+// Copyright 2025 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 <string>
+#include <memory>
+#include <librealsense2/rs.hpp>
+#include <sensor_params.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <ros_sensor.h>
+#include "named_filter.h"
+
+namespace realsense2_camera
+{
+    class PointcloudFilter : public NamedFilter
+    {
+        public:
+            PointcloudFilter(std::shared_ptr<rs2::filter> filter, rclcpp::Node& node, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled=false);
+
+            void setPublisher();
+            void Publish(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset, const std::string& frame_id);
+
+        private:
+            void setParameters();
+
+        private:
+            bool _is_enabled_pc;
+            rclcpp::Node& _node;
+            bool _allow_no_texture_points;
+            bool _ordered_pc;
+            std::mutex _mutex_publisher;
+            rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _pointcloud_publisher;
+            std::string _pointcloud_qos;
+    };
+}

realsense2_camera/src/align_depth_filter.cpp

@@ -0,0 +1,29 @@
+// Copyright 2025 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 <align_depth_filter.h>
+
+
+using namespace realsense2_camera;
+
+
+AlignDepthFilter::AlignDepthFilter(std::shared_ptr<rs2::filter> filter,
+    std::function<void(const rclcpp::Parameter&)> update_align_depth_func,
+    std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled):
+    NamedFilter(filter, parameters, logger, is_enabled, false)
+{
+    _params.registerDynamicOptions(*(_filter.get()), "align_depth");
+    _params.getParameters()->setParamT("align_depth.enable", _is_enabled, update_align_depth_func);
+    _parameters_names.push_back("align_depth.enable");
+}

realsense2_camera/src/base_realsense_node.cpp

@@ -23,6 +23,8 @@
 // Header files for disabling intra-process comms for static broadcaster.
 #include <rclcpp/publisher_options.hpp>
 #include <tf2_ros/qos.hpp>
+#include "pointcloud_filter.h"
+#include "align_depth_filter.h"
 
 using namespace realsense2_camera;
 
@@ -598,6 +600,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
         }
 
         rs2::video_frame original_color_frame = frameset.get_color_frame();
+        rs2::video_frame original_infra2_frame = frameset.get_infrared_frame(2);
 
         ROS_DEBUG("num_filters: %d", static_cast<int>(_filters.size()));
         for (auto filter_it : _filters)
@@ -635,6 +638,11 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
                         publishFrame(f, t, COLOR, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, false);
                         continue;
                     }
+                    if (original_infra2_frame && _align_depth_filter->is_enabled())
+                    {
+                        publishFrame(f, t, INFRA2, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, false);
+                        continue;
+                    }
                 }
                 publishFrame(f, t, sip, _images, _info_publishers, _image_publishers);
             }