Added frameskip pointcloud feature

pal-robotics-forks · Jan 19, 2024 · 3580b30 · 3580b30
1 parent bee988f
commit 3580b30
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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)
             {