Adding more precise scan separation feature

velodyne_pointcloud/include/velodyne_pointcloud/convert.h

@@ -88,6 +88,12 @@ class Convert
     Config config_;
     bool first_rcfg_call;
 
+    // Additional parameters for checking azimuth differences between consecutive data blocks
+    velodyne_msgs::VelodyneScanPtr adjusted_vel_msg_ptr {new velodyne_msgs::VelodyneScan};
+    int last_azimuth_;
+    int current_azimuth_diff_;
+    int prev_azimuth_diff_;
+
   // diagnostics updater
   diagnostic_updater::Updater diagnostics_;
   double diag_min_freq_;

velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h

@@ -39,6 +39,7 @@
 #include <string>
 #include <algorithm>
 #include <cstdarg>
+#include <vector>
 
 namespace velodyne_rawdata
 {
@@ -113,6 +114,7 @@ class DataContainerBase
     cloud.width = config_.init_width;
     cloud.height = config_.init_height;
     cloud.is_dense = static_cast<uint8_t>(config_.is_dense);
+    is_newscan = false;
   }
 
   virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance,
@@ -150,6 +152,49 @@ class DataContainerBase
 
   sensor_msgs::PointCloud2 cloud;
 
+  bool is_newscan;
+  int last_azimuth_corrected;
+  int current_corrected_azimuth_diff;
+  int prev_corrected_azimuth_diff;
+
+  // Struct for residual points
+  struct RPoint
+  {
+      float x;
+      float y;
+      float z;
+      uint16_t ring;
+      uint16_t azimuth;
+      float distance;
+      float intensity;
+  };
+
+  std::vector<RPoint> residual_cloud_;
+
+  void addResidualPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance,
+                        const float intensity)
+  {
+    RPoint point = {x, y, z, ring, azimuth, distance, intensity};
+    residual_cloud_.push_back(point);
+  }
+
+  void offloadResidualPoint()
+  {
+    // Add residual points from the previous scan
+    for (size_t p = 0; p < residual_cloud_.size(); ++p)
+    {
+        addPoint(residual_cloud_[p].x, residual_cloud_[p].y, residual_cloud_[p].z,
+                residual_cloud_[p].ring, residual_cloud_[p].azimuth, residual_cloud_[p].distance,
+                residual_cloud_[p].intensity);
+    }
+    residual_cloud_.clear();
+  }
+
+  void addOffsetDuration(ros::Duration offset)
+  {
+    cloud.header.stamp = cloud.header.stamp + offset;
+  }
+
   inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3f& eigen_vec)
   {
     eigen_vec(0) = tf_vec[0];

velodyne_pointcloud/src/conversions/convert.cc

@@ -51,6 +51,14 @@ namespace velodyne_pointcloud
               data_->scansPerPacket()));
     }
 
+    // Initilize all parameters used for data block azimuth difference calculation
+    last_azimuth_ = -1;
+    current_azimuth_diff_ = -1;
+    prev_azimuth_diff_ = -1;
+
+    container_ptr_->last_azimuth_corrected = -1;
+    container_ptr_->current_corrected_azimuth_diff = -1;
+    container_ptr_->prev_corrected_azimuth_diff = -1;
 
     // advertise output point cloud (before subscribing to input data)
     output_ =
@@ -131,11 +139,56 @@ namespace velodyne_pointcloud
     // process each packet provided by the driver
     for (size_t i = 0; i < scanMsg->packets.size(); ++i)
     {
-      data_->unpack(scanMsg->packets[i], *container_ptr_);
+        velodyne_msgs::VelodynePacket tmp_packet = scanMsg->packets[i];
+
+        // Extract base rotation of first block in packet
+        std::size_t azimuth_data_pos = 100*0+2;
+        int azimuth = *( (u_int16_t*) (&tmp_packet.data[azimuth_data_pos]));
+
+        //if first packet in scan, there is no "valid" last_azimuth_
+        if (last_azimuth_ == -1)
+        {
+            last_azimuth_ = azimuth;
+            continue;
+        }
+        if (current_azimuth_diff_ == -1)
+        {
+            current_azimuth_diff_ = azimuth - last_azimuth_;
+            continue;
+        }
+        if (prev_azimuth_diff_ == -1)
+        {
+            prev_azimuth_diff_ = current_azimuth_diff_;
+            continue;
+        }
+        current_azimuth_diff_ = azimuth - last_azimuth_;
+
+        adjusted_vel_msg_ptr->packets.push_back(tmp_packet);
+
+        if ((current_azimuth_diff_ < 0) && (prev_azimuth_diff_ >= 0))
+        {
+            // Current and previous azimuth differences have the same sign indicating the current packet was taken at 0 deg
+            adjusted_vel_msg_ptr->header.stamp = tmp_packet.stamp;
+
+            // allocate a point cloud with same time and frame ID as raw data
+            container_ptr_->setup(adjusted_vel_msg_ptr);
+            container_ptr_->offloadResidualPoint();
+
+            // process each packet
+            for (size_t j = 0; j < adjusted_vel_msg_ptr->packets.size(); ++j)
+            {
+              data_->unpack(adjusted_vel_msg_ptr->packets[j], *container_ptr_);
+            }
+
+            // Clear all the converted packets
+            adjusted_vel_msg_ptr->packets.clear();
+        }
+        last_azimuth_ = azimuth;
+        prev_azimuth_diff_ = current_azimuth_diff_;
     }
 
     // publish the accumulated cloud message
-    diag_topic_->tick(scanMsg->header.stamp);
+    diag_topic_->tick(adjusted_vel_msg_ptr->header.stamp);
     diagnostics_.update();
     output_.publish(container_ptr_->finishCloud());
   }

velodyne_pointcloud/src/lib/rawdata.cc

@@ -379,7 +379,23 @@ inline float SQR(float val) { return val*val; }
           /** correct for the laser rotation as a function of timing during the firings **/
           azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*VLP16_DSR_TOFFSET) + (firing*VLP16_FIRING_TOFFSET)) / VLP16_BLOCK_TDURATION);
           azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;
-
+
+          // Calculate azimuth difference of three consecutive scan points
+          if (data.last_azimuth_corrected == -1)
+          {
+            data.last_azimuth_corrected = azimuth_corrected;
+          }
+          else if (data.current_corrected_azimuth_diff == -1)
+          {
+          	data.current_corrected_azimuth_diff = azimuth_corrected - data.last_azimuth_corrected;
+          }
+          else if (data.prev_corrected_azimuth_diff == -1)
+      	  {
+          	data.prev_corrected_azimuth_diff = data.current_corrected_azimuth_diff;
+          }
+
+          data.current_corrected_azimuth_diff = azimuth_corrected - data.last_azimuth_corrected;
+
           /*condition added to avoid calculating points which are not
             in the interesting defined area (min_angle < area < max_angle)*/
           if ((azimuth_corrected >= config_.min_angle 
@@ -484,8 +500,26 @@ inline float SQR(float val) { return val*val; }
               SQR(1 - tmp.uint/65535)));
             intensity = (intensity < min_intensity) ? min_intensity : intensity;
             intensity = (intensity > max_intensity) ? max_intensity : intensity;
-
-            data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity);
+
+            if ((data.current_corrected_azimuth_diff < 0) && (data.prev_corrected_azimuth_diff) >= 0)
+            {
+                data.is_newscan = true;
+            }
+            data.last_azimuth_corrected = azimuth_corrected;
+            data.prev_corrected_azimuth_diff = data.current_corrected_azimuth_diff;
+
+            if (data.is_newscan)
+            {
+                data.addResidualPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity);
+            }
+            else
+            {
+                data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity);
+                // Update timestamp
+                ros::Duration first_block_time_offset(0, block*((dsr * VLP16_DSR_TOFFSET) +
+                                                                 (firing * VLP16_FIRING_TOFFSET))); // [µs]
+                data.addOffsetDuration(first_block_time_offset);
+            }
           }
         }
         data.newLine();