From 0305426edab7cab8d809c4e51557d0c051c2d132 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Thu, 12 Nov 2020 17:49:54 +0100 Subject: [PATCH] add container constructor for PointcloudXYZIR enabling padding --- .../include/velodyne_pointcloud/convert.h | 4 +++- .../include/velodyne_pointcloud/point_types.h | 6 ++---- .../velodyne_pointcloud/pointcloudXYZIR.h | 4 ++++ .../include/velodyne_pointcloud/transform.h | 4 +++- .../src/conversions/convert.cc | 5 +++++ .../src/conversions/pointcloudXYZIR.cc | 20 ++++++++++++++++++- .../src/conversions/transform.cc | 5 +++++ 7 files changed, 41 insertions(+), 7 deletions(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h index a6a8d1e97..eb4c24fa2 100644 --- 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 container_names_ { + {"PointCloudXYZPIR", Container::PointCloudXYZPIR}, {"PointCloudXYZIR", Container::PointCloudXYZIR}, {"PointCloudXYZIRT", Container::PointCloudXYZIRT}, {"OrganizedPointCloudXYZIR", Container::OrganizedPointCloudXYZIR}, {"OrganizedPointCloudXYZIRT", Container::OrganizedPointCloudXYZIRT} }; - const std::pair default_container_ = {"PointCloudXYZIR", PointCloudXYZIR}; + const std::pair default_container_ = {"PointCloudXYZPIR", PointCloudXYZPIR}; const std::pair 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 index 613be32fd..5ced8dfa7 100644 --- 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 index e5d60414d..bef4fca9c 100644 --- 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_ptr = boost::shared_ptr()); + 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_ptr = boost::shared_ptr()); + 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 index 5a7ec4a59..361de8f11 100644 --- 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 container_names_ { + {"PointCloudXYZPIR", Container::PointCloudXYZPIR}, {"PointCloudXYZIR", Container::PointCloudXYZIR}, {"PointCloudXYZIRT", Container::PointCloudXYZIRT}, {"OrganizedPointCloudXYZIR", Container::OrganizedPointCloudXYZIR}, {"OrganizedPointCloudXYZIRT", Container::OrganizedPointCloudXYZIRT} }; - const std::pair default_container_ = {"PointCloudXYZIR", PointCloudXYZIR}; + const std::pair default_container_ = {"PointCloudXYZPIR", PointCloudXYZPIR}; const std::pair default_organized_container_ = {"OrganizedPointCloudXYZIR", OrganizedPointCloudXYZIR}; diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc index e05f1e4b0..cfd0d7f17 100644 --- 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( + new PointcloudXYZIR("p", config_.max_range, config_.min_range, + config_.target_frame, config_.fixed_frame, + data_->scansPerPacket())); case Container::PointCloudXYZIR: return boost::shared_ptr( new PointcloudXYZIR(config_.max_range, config_.min_range, diff --git a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc index e212843ba..caa3eb9d9 100644 --- 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_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(cloud, "x"); iter_y = sensor_msgs::PointCloud2Iterator(cloud, "y"); diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 7cf156703..06f8d7188 100644 --- 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( + 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( new PointcloudXYZIR(config_.max_range, config_.min_range,