velodyne_packet_pointcloud: Dual return WIP

UniBwTAS · Aug 9, 2024 · 8e6c320 · 8e6c320
1 parent 4cd96e4
commit 8e6c320
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 4 deletions.
diff --git a/velodyne_packet_pointcloud/src/unpacker.cpp b/velodyne_packet_pointcloud/src/unpacker.cpp
@@ -114,13 +114,13 @@ namespace velodyne_packet_pointcloud
 
 
         manage_tf_buffer();
+        packets_in_scan = 1; // must be ste to at least one
         cloud.header.stamp = packetMsg.stamp;
         cloud.data.clear();
         cloud.data.resize(points_per_packet * cloud.point_step);
         cloud.width = 1;
         cloud.height = 0;// starts at 0 and increases with each succes full call to add point
-        cloud.is_dense = true;
-
+        cloud.is_dense = false;
 
         iter_x = sensor_msgs::PointCloud2Iterator<float>(cloud, "x");
         iter_y = sensor_msgs::PointCloud2Iterator<float>(cloud, "y");
@@ -147,9 +147,13 @@ namespace velodyne_packet_pointcloud
             // fixed frame not available
             return;
         }
+        ROS_DEBUG_STREAM("Unpacking" << cloud.height * cloud.width
+                                      << " Velodyne points, time: " << cloud.header.stamp);
 
-        raw_data_ptr_->unpack(packetMsg, *this,
-                              packetMsg.stamp, 0);
+        raw_data_ptr_->unpack(packetMsg, *this,packetMsg.stamp,
+                              0); // todo: this may have to be a running number
+        ROS_DEBUG_STREAM("Publishing" << cloud.height * cloud.width
+                                      << " Velodyne points, time: " << cloud.header.stamp);
 
 
         output_.publish(this->finishCloud(packetMsg.stamp));
@@ -159,6 +163,7 @@ namespace velodyne_packet_pointcloud
         if (this->get_return_mode(retmode_msg))
             output_ret_mode_.publish(retmode_msg);
 
+
     }
 
     void

diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc
@@ -753,6 +753,7 @@ void RawData::unpack_vls128(const velodyne_msgs::VelodynePacket &pkt, DataContai
               float point_time = 0;
 
               //To which firing seq does this block belong in the package
+              // In dual return there is only 1 fire sequence per package, so this is always the first one (0)
               const uint8_t  firing_seq_in_package = 0;
 
               // To which firing sequence does this block belongs in the scan
@@ -817,6 +818,7 @@ void RawData::unpack_vls128(const velodyne_msgs::VelodynePacket &pkt, DataContai
                   std::uint16_t rotation_segment = ((7 - (laser_number - (8 * std::floor(laser_number / 8)))) * 9) +
                                                    firing_seq_in_scan;
 
+                  // ToDo fix for continuous streaming
                   while (rotation_segment >= num_firing_sequences_in_one_scan) {
                       rotation_segment = rotation_segment - num_firing_sequences_in_one_scan;
                   }