From 3580b30259a7e5bbb6981b10105d8d7fe783f2ab Mon Sep 17 00:00:00 2001
From: sergiomoyano <sergio.moyano@pal-robotics.com>
Date: Fri, 19 Jan 2024 15:35:41 +0100
Subject: [PATCH] Added frameskip pointcloud feature

---
 realsense2_camera/include/base_realsense_node.h | 2 +-
 realsense2_camera/include/constants.h           | 3 ++-
 realsense2_camera/launch/rs_launch.py           | 4 +++-
 realsense2_camera/src/base_realsense_node.cpp   | 9 +++++++--
 4 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
index cd9aaba667..f59339665e 100755
--- a/realsense2_camera/include/base_realsense_node.h
+++ b/realsense2_camera/include/base_realsense_node.h
@@ -320,7 +320,7 @@ namespace realsense2_camera
         std::shared_ptr<diagnostic_updater::Updater> _diagnostics_updater;
         rs2::stream_profile _base_profile;
 
-
+        int _pointcloud_frame_skip;
     };//end class
 }
 
diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h
index e5ec5a2413..5357429af1 100644
--- a/realsense2_camera/include/constants.h
+++ b/realsense2_camera/include/constants.h
@@ -107,7 +107,8 @@ namespace realsense2_camera
     const bool ENABLE_IMU     = true;
     const bool HOLD_BACK_IMU_FOR_FRAMES = false;
     const bool PUBLISH_ODOM_TF = true;
-
+    
+    const int POINTCLOUD_FRAME_SKIP = 1;
 
     const std::string DEFAULT_BASE_FRAME_ID            = "link";
     const std::string DEFAULT_ODOM_FRAME_ID            = "odom_frame";
diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py
index db633ead9f..edd05154be 100644
--- a/realsense2_camera/launch/rs_launch.py
+++ b/realsense2_camera/launch/rs_launch.py
@@ -73,7 +73,9 @@
                            {'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'},
-                          ]
+                           {'name': 'pointcloud_frame_skip',        'default': '1', 'description': 'Pointcloud framerate skip divider'},
+                           
+                    ]
 
 def declare_configurable_parameters(parameters):
     return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters]
diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
index 6da4d648fc..5748d4b6c0 100755
--- a/realsense2_camera/src/base_realsense_node.cpp
+++ b/realsense2_camera/src/base_realsense_node.cpp
@@ -113,7 +113,8 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
     _publish_odom_tf(false),
     _imu_sync_method(imu_sync_method::NONE),
     _is_profile_changed(false),
-    _is_align_depth_changed(false)
+    _is_align_depth_changed(false),
+    _pointcloud_frame_skip(1)
 {
     if ( use_intra_process )
     {
@@ -525,8 +526,12 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
 
             if (f.is<rs2::points>())
             {
-                publishPointCloud(f.as<rs2::points>(), t, frameset);
+                if (frame.get_frame_number() % _pointcloud_frame_skip == 0)
+                {
+                    publishPointCloud(f.as<rs2::points>(), t, frameset);
+                }
                 continue;
+                
             }
             if (stream_type == RS2_STREAM_DEPTH)
             {