From 7904898b49b848ca5c7d6071ad71c6a14dfcf4af Mon Sep 17 00:00:00 2001
From: jugo <juan.gonzalez@unibw.de>
Date: Mon, 22 Jul 2024 17:08:19 +0200
Subject: [PATCH] velodyne_pointcloud: read the return mode flag in every
 Ethernet packet to catch a change between two modes and discard the
 accumulated packages.

---
 .../include/velodyne_pointcloud/transform.h   |  1 +
 .../src/conversions/transform.cc              | 29 +++++++++++++------
 2 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h
index ff40b29e1..b9e1ca344 100644
--- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h
+++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h
@@ -126,6 +126,7 @@ class Transform
   int _last_azimuth{-1};
   bool _first_rotation{true};
   double _rpm{600.0};
+  uint8_t _last_return_mode{0};
 };
 }  // namespace velodyne_pointcloud
 
diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc
index 2404310b9..0864663ff 100644
--- a/velodyne_pointcloud/src/conversions/transform.cc
+++ b/velodyne_pointcloud/src/conversions/transform.cc
@@ -260,9 +260,25 @@ namespace velodyne_pointcloud
             return;
         }
 
-        if ((_last_azimuth < _cut_angle && _cut_angle <= azimuth) ||
-            (azimuth < _last_azimuth && _cut_angle <= azimuth) ||
-            (azimuth < _last_azimuth && _last_azimuth < _cut_angle)) {
+        // get if the packet is single or double return
+
+        std::size_t ret_mode_data_pos = velodyne_rawdata::BLOCK_SIZE * velodyne_rawdata::BLOCKS_PER_PACKET + velodyne_rawdata::TIMESTAMP_SIZE;
+        uint8_t mode = 0; // 55 .. 58
+        std::memcpy(&mode, &ethernet_msg->payload[ret_mode_data_pos], velodyne_rawdata::RETURN_MODE_BYTE_SIZE);
+
+        // Compare with the return mode of the previous package to detect a change
+        if (_last_return_mode != mode)
+        {
+            // Change in the return mode from previous package
+            // set first rotation flag to tru to discard the current polled packages, as they are
+            // a combination of packages taken with different return modes
+            _first_rotation = true;
+            _last_return_mode = mode;
+        }
+
+        if ((_last_azimuth < _cut_angle && _cut_angle <= azimuth) || //  e.g. la 50,  ca 60,  a 70
+            (azimuth < _last_azimuth && _cut_angle <= azimuth) ||    //  e.g. la 350, ca 5,   a 10
+            (azimuth < _last_azimuth && _last_azimuth < _cut_angle)) {// e.g. la 340, ca 359, a 15
 
             if (_first_rotation) { // discard the first accumulation of packets as it
                 // does not build a complete rotation
@@ -275,13 +291,8 @@ namespace velodyne_pointcloud
 
                 return;
             }
-            // get if the packet is single or double return
-
-            std::size_t ret_mode_data_pos = velodyne_rawdata::BLOCK_SIZE * velodyne_rawdata::BLOCKS_PER_PACKET + velodyne_rawdata::TIMESTAMP_SIZE;
-            uint8_t mode = 0; // 55 .. 58
-            std::memcpy(&mode, &ethernet_msg->payload[ret_mode_data_pos], velodyne_rawdata::RETURN_MODE_BYTE_SIZE);
-
 
+            
             const double frequency = (_rpm / 60.0);
             if (mode == velodyne_rawdata::VLS128_RETURN_MODE_STRONGEST || mode == velodyne_rawdata::VLS128_RETURN_MODE_LAST)
                 _npackets = (int) ceil(velodyne_rawdata::PACKET_RATE_SINGLE_RET_MODE / frequency); // packets per rev