PR #2336 from Samer: Fix colorizer filter which returns null referenc…

…e ptr
IntelRealSense · May 2, 2022 · 68396a8 · 68396a8
2 parents eb93433 + a7e7deb
commit 68396a8
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 2 deletions.
diff --git a/realsense2_camera/include/named_filter.h b/realsense2_camera/include/named_filter.h
@@ -18,6 +18,7 @@ namespace realsense2_camera
             NamedFilter(std::shared_ptr<rs2::filter> filter, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled=false, bool is_set_parameters=true);
             bool is_enabled() {return _is_enabled;};
             rs2::frameset Process(rs2::frameset frameset);
+            rs2::frame Process(rs2::frame frame);
 
         protected:
             void setParameters(std::function<void(const rclcpp::Parameter&)> enable_param_func = std::function<void(const rclcpp::Parameter&)>());

diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
@@ -601,9 +601,13 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
             }
             if (original_depth_frame && _align_depth_filter->is_enabled())
             {
+                rs2::frame frame_to_send;
                 if (_colorizer_filter->is_enabled())
-                    original_depth_frame = _colorizer_filter->Process(original_depth_frame);
-                publishFrame(original_depth_frame, t,
+                    frame_to_send = _colorizer_filter->Process(original_depth_frame);
+                else
+                    frame_to_send = original_depth_frame;
+
+                publishFrame(frame_to_send, t,
                                 DEPTH,
                                 _image,
                                 _info_publisher,

diff --git a/realsense2_camera/src/named_filter.cpp b/realsense2_camera/src/named_filter.cpp
@@ -49,6 +49,18 @@ rs2::frameset NamedFilter::Process(rs2::frameset frameset)
     }
 }
 
+rs2::frame NamedFilter::Process(rs2::frame frame)
+{
+    if (_is_enabled)
+    {
+        return _filter->process(frame);
+    }
+    else
+    {
+        return frame;
+    }
+}
+
 
 PointcloudFilter::PointcloudFilter(std::shared_ptr<rs2::filter> filter, rclcpp::Node& node, std::shared_ptr<Parameters> parameters, rclcpp::Logger logger, bool is_enabled):
     NamedFilter(filter, parameters, logger, is_enabled, false),