From 431e279aef2c35d0a6a3f9d7b16c86d069790783 Mon Sep 17 00:00:00 2001
From: Ray Zhang <>
Date: Fri, 12 Apr 2024 11:40:18 +0800
Subject: [PATCH 1/2] fix angle calculation

---
 src/pointcloud_to_laserscan_nodelet.cpp | 42 +++++++++++++++++++------
 1 file changed, 32 insertions(+), 10 deletions(-)

diff --git a/src/pointcloud_to_laserscan_nodelet.cpp b/src/pointcloud_to_laserscan_nodelet.cpp
index 184ff7e..98fe0cb 100644
--- a/src/pointcloud_to_laserscan_nodelet.cpp
+++ b/src/pointcloud_to_laserscan_nodelet.cpp
@@ -195,6 +195,7 @@ void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPt
     cloud_out = cloud_msg;
   }
 
+  int n=0;
   // Iterate through pointcloud
   for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_out, "x"), iter_y(*cloud_out, "y"),
        iter_z(*cloud_out, "z");
@@ -226,20 +227,41 @@ void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPt
       continue;
     }
 
-    double angle = atan2(*iter_y, *iter_x);
-    if (angle < output.angle_min || angle > output.angle_max)
+    double angle_origin = atan2(*iter_y, *iter_x);
+    // Transforms the angle into the (angle_min, angle_max) range.
+    double angle = angle_origin + n * 2 * M_PI;
+    while (angle < output.angle_min)
     {
-      NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
-      continue;
+      ++n;
+      angle = angle_origin + n * 2 * M_PI;
     }
-
-    // overwrite range at laserscan ray if new range is smaller
-    int index = (angle - output.angle_min) / output.angle_increment;
-    if (range < output.ranges[index])
-    {
-      output.ranges[index] = range;
+    if(angle <= output.angle_max){
+      // overwrite range at laserscan ray if new range is smaller
+      int index = (angle - output.angle_min) / output.angle_increment;
+      if (range < output.ranges[index])
+      {
+        output.ranges[index] = range;
+      }
+      continue;
+    }else{
+      while(angle > output.angle_max){
+        --n;
+        angle = angle_origin + n * 2 * M_PI;
+      }
+      if(angle>=output.angle_min){
+        // overwrite range at laserscan ray if new range is smaller
+        int index = (angle - output.angle_min) / output.angle_increment;
+        if (range < output.ranges[index])
+        {
+          output.ranges[index] = range;
+        }
+        continue;
+      }
     }
+    
+    NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
   }
+
   pub_.publish(output);
 }
 }  // namespace pointcloud_to_laserscan

From c2125f4952ccb6903c74f12a70e76112b820fe18 Mon Sep 17 00:00:00 2001
From: Ray Zhang <>
Date: Thu, 18 Apr 2024 14:45:07 +0800
Subject: [PATCH 2/2] fixed

---
 src/pointcloud_to_laserscan_nodelet.cpp | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/pointcloud_to_laserscan_nodelet.cpp b/src/pointcloud_to_laserscan_nodelet.cpp
index 98fe0cb..dbca819 100644
--- a/src/pointcloud_to_laserscan_nodelet.cpp
+++ b/src/pointcloud_to_laserscan_nodelet.cpp
@@ -195,7 +195,7 @@ void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPt
     cloud_out = cloud_msg;
   }
 
-  int n=0;
+  int n = 0;
   // Iterate through pointcloud
   for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_out, "x"), iter_y(*cloud_out, "y"),
        iter_z(*cloud_out, "z");
@@ -235,7 +235,8 @@ void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPt
       ++n;
       angle = angle_origin + n * 2 * M_PI;
     }
-    if(angle <= output.angle_max){
+    if (angle <= output.angle_max)
+    {
       // overwrite range at laserscan ray if new range is smaller
       int index = (angle - output.angle_min) / output.angle_increment;
       if (range < output.ranges[index])
@@ -243,12 +244,16 @@ void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPt
         output.ranges[index] = range;
       }
       continue;
-    }else{
-      while(angle > output.angle_max){
+    }
+    else
+    {
+      while (angle > output.angle_max)
+      {
         --n;
         angle = angle_origin + n * 2 * M_PI;
       }
-      if(angle>=output.angle_min){
+      if (angle >= output.angle_min)
+      {
         // overwrite range at laserscan ray if new range is smaller
         int index = (angle - output.angle_min) / output.angle_increment;
         if (range < output.ranges[index])
@@ -258,7 +263,6 @@ void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPt
         continue;
       }
     }
-    
     NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
   }