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 _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()) { - publishPointCloud(f.as(), t, frameset); + if (frame.get_frame_number() % _pointcloud_frame_skip == 0) + { + publishPointCloud(f.as(), t, frameset); + } continue; + } if (stream_type == RS2_STREAM_DEPTH) {