diff --git a/ldlidar_driver/src/log_module.cpp b/ldlidar_driver/src/log_module.cpp
index 61369e0..439708d 100644
--- a/ldlidar_driver/src/log_module.cpp
+++ b/ldlidar_driver/src/log_module.cpp
@@ -27,6 +27,9 @@
 #include <stdlib.h>
 #endif
 
+// Ubuntu 24.04 won't compile without this include:
+#include <pthread.h>
+
 //使用vswprintf会出现奔溃的情况如果，传入数据大于 VA_PARAMETER_MAX 就会出现崩溃
 #define  VA_PARAMETER_MAX  (1024 * 2)
 
@@ -281,4 +284,4 @@ void LogPrint::LogPrintInf(const char* str) {
 #endif
 }
 
-/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/
\ No newline at end of file
+/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/
diff --git a/src/demo.cpp b/src/demo.cpp
index 4efdda8..652cbb3 100644
--- a/src/demo.cpp
+++ b/src/demo.cpp
@@ -48,6 +48,10 @@ int main(int argc, char **argv) {
   int serial_baudrate = 0;
   ldlidar::LDType lidartypename = ldlidar::LDType::NO_VER;
 
+  // Added to measure average beam count (points per revolution) at start:
+  int beam_count = 0;
+  int beam_count_i = 0;
+
   // declare ros2 param
   node->declare_parameter<std::string>("product_name", product_name);
   node->declare_parameter<std::string>("laser_scan_topic_name", laser_scan_topic_name);
@@ -137,8 +141,25 @@ int main(int argc, char **argv) {
       case ldlidar::LidarStatus::NORMAL: {
         double lidar_scan_freq = 0;
         lidar_drv->GetLidarScanFreq(lidar_scan_freq);
-        ToLaserscanMessagePublish(laser_scan_points, lidar_scan_freq, setting, node, lidar_pub_laserscan);
-        ToSensorPointCloudMessagePublish(laser_scan_points, setting, node, lidar_pub_pointcloud);
+        int n_points = static_cast<int>(laser_scan_points.size());
+        int n_samples = 20;
+        if(beam_count_i++ < n_samples) {
+          // Measure average beam count (points per revolution) at start:
+          if(beam_count_i > 1) {
+            // skip the first sample, it is messed up
+            beam_count += n_points;
+          }
+          //RCLCPP_INFO(node->get_logger(), "beam count: %d", n_points);
+          if(beam_count_i == n_samples) {
+            beam_count = beam_count / (n_samples - 1);
+            RCLCPP_INFO(node->get_logger(), "Average beam count: %d", beam_count);
+          }
+        } else if(n_points > beam_count - 5) {
+          // ensure the size of points vector is constant between LIDAR head revolutions:
+          laser_scan_points.resize(beam_count, laser_scan_points.back());
+          ToLaserscanMessagePublish(laser_scan_points, lidar_scan_freq, setting, node, lidar_pub_laserscan);
+          ToSensorPointCloudMessagePublish(laser_scan_points, setting, node, lidar_pub_pointcloud);
+        }
         break;
       }
       case ldlidar::LidarStatus::DATA_TIME_OUT: {