add container constructor for PointcloudXYZIR enabling padding

ros-drivers · Dec 4, 2022 · 0305426 · 0305426
1 parent e81f532
commit 0305426
Show file tree

Hide file tree

Showing 7 changed files with 41 additions and 7 deletions.
diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h
@@ -80,6 +80,7 @@ class Convert
 
     enum Container
     {
+      PointCloudXYZPIR,
       PointCloudXYZIR,
       PointCloudXYZIRT,
       OrganizedPointCloudXYZIR,
@@ -88,13 +89,14 @@ class Convert
 
     std::map<std::string, Container> container_names_
     {
+      {"PointCloudXYZPIR", Container::PointCloudXYZPIR},
       {"PointCloudXYZIR", Container::PointCloudXYZIR},
       {"PointCloudXYZIRT", Container::PointCloudXYZIRT},
       {"OrganizedPointCloudXYZIR", Container::OrganizedPointCloudXYZIR},
       {"OrganizedPointCloudXYZIRT", Container::OrganizedPointCloudXYZIRT}
     };
 
-    const std::pair<std::string, uint8_t> default_container_ = {"PointCloudXYZIR", PointCloudXYZIR};
+    const std::pair<std::string, uint8_t> default_container_ = {"PointCloudXYZPIR", PointCloudXYZPIR};
     const std::pair<std::string, uint8_t> default_organized_container_ =
         {"OrganizedPointCloudXYZIR", OrganizedPointCloudXYZIR};
 

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/point_types.h b/velodyne_pointcloud/include/velodyne_pointcloud/point_types.h
@@ -26,12 +26,10 @@ namespace velodyne_pointcloud
 /** Euclidean Velodyne coordinate, including intensity and ring number. */
 struct PointXYZIR
 {
-  float x;                         ///< x coord
-  float y;                         ///< y coord
-  float z;                         ///< z coord
+  PCL_ADD_POINT4D;                 ///< quad-word XYZ
   float intensity;                 ///< laser intensity reading
   uint16_t ring;                   ///< laser ring number
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // ensure proper alignment
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  ///< ensure proper alignment
 }
 EIGEN_ALIGN16;
 

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h
@@ -45,6 +45,10 @@ class PointcloudXYZIR : public velodyne_rawdata::DataContainerBase
                   const std::string& fixed_frame, const unsigned int scans_per_block,
                   boost::shared_ptr<tf::TransformListener> tf_ptr = boost::shared_ptr<tf::TransformListener>());
 
+  PointcloudXYZIR(std::string pad, const double max_range, const double min_range, const std::string& target_frame,
+                  const std::string& fixed_frame, const unsigned int scans_per_block,
+                  boost::shared_ptr<tf::TransformListener> tf_ptr = boost::shared_ptr<tf::TransformListener>());
+
   virtual void newLine();
 
   virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg);

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h
@@ -90,6 +90,7 @@ class Transform
 
   enum Container
   {
+    PointCloudXYZPIR,
     PointCloudXYZIR,
     PointCloudXYZIRT,
     OrganizedPointCloudXYZIR,
@@ -98,13 +99,14 @@ class Transform
 
   std::map<std::string, Container> container_names_
   {
+    {"PointCloudXYZPIR", Container::PointCloudXYZPIR},
     {"PointCloudXYZIR", Container::PointCloudXYZIR},
     {"PointCloudXYZIRT", Container::PointCloudXYZIRT},
     {"OrganizedPointCloudXYZIR", Container::OrganizedPointCloudXYZIR},
     {"OrganizedPointCloudXYZIRT", Container::OrganizedPointCloudXYZIRT}
   };
 
-  const std::pair<std::string, uint8_t> default_container_ = {"PointCloudXYZIR", PointCloudXYZIR};
+  const std::pair<std::string, uint8_t> default_container_ = {"PointCloudXYZPIR", PointCloudXYZPIR};
   const std::pair<std::string, uint8_t> default_organized_container_ =
       {"OrganizedPointCloudXYZIR", OrganizedPointCloudXYZIR};
 

diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc
@@ -127,6 +127,11 @@ namespace velodyne_pointcloud
   {
     switch (container_id)
     {
+      case Container::PointCloudXYZPIR:
+        return boost::shared_ptr<PointcloudXYZIR>(
+            new PointcloudXYZIR("p", config_.max_range, config_.min_range,
+                                config_.target_frame, config_.fixed_frame,
+                                data_->scansPerPacket()));
       case Container::PointCloudXYZIR:
         return boost::shared_ptr<PointcloudXYZIR>(
             new PointcloudXYZIR(config_.max_range, config_.min_range,

diff --git a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc
@@ -21,7 +21,25 @@ namespace velodyne_pointcloud
         iter_ring(cloud, "ring"), iter_intensity(cloud, "intensity")
     {};
 
-  void PointcloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){
+  PointcloudXYZIR::PointcloudXYZIR(std::string pad,
+    const double max_range, const double min_range,
+    const std::string& target_frame, const std::string& fixed_frame,
+    const unsigned int scans_per_block, boost::shared_ptr<tf::TransformListener> tf_ptr)
+    : DataContainerBase(
+    max_range, min_range, target_frame, fixed_frame,
+    0, 1, true, scans_per_block, tf_ptr, 6,
+    "x", 1, sensor_msgs::PointField::FLOAT32,
+    "y", 1, sensor_msgs::PointField::FLOAT32,
+    "z", 1, sensor_msgs::PointField::FLOAT32,
+    pad, 1, sensor_msgs::PointField::FLOAT32,
+    "intensity", 1, sensor_msgs::PointField::FLOAT32,
+    "ring", 1, sensor_msgs::PointField::UINT16),
+      iter_x(cloud, "x"), iter_y(cloud, "y"), iter_z(cloud, "z"),
+      iter_ring(cloud, "ring"), iter_intensity(cloud, "intensity")
+    {};
+
+  void PointcloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg)
+  {
     DataContainerBase::setup(scan_msg);
     iter_x = sensor_msgs::PointCloud2Iterator<float>(cloud, "x");
     iter_y = sensor_msgs::PointCloud2Iterator<float>(cloud, "y");

diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc
@@ -128,6 +128,11 @@ namespace velodyne_pointcloud
   {
     switch (container_id)
     {
+      case Container::PointCloudXYZPIR:
+        return boost::shared_ptr<PointcloudXYZIR>(
+            new PointcloudXYZIR("p", config_.max_range, config_.min_range,
+                                config_.target_frame, config_.fixed_frame,
+                                data_->scansPerPacket(), tf_ptr_));
       case Container::PointCloudXYZIR:
         return boost::shared_ptr<PointcloudXYZIR>(
             new PointcloudXYZIR(config_.max_range, config_.min_range,