00001
00002 #include <velodyne_pointcloud/organized_cloudXYZIR.h>
00003
00004 namespace velodyne_pointcloud
00005 {
00006
00007 OrganizedCloudXYZIR::OrganizedCloudXYZIR(
00008 const double max_range, const double min_range,
00009 const std::string& target_frame, const std::string& fixed_frame,
00010 const unsigned int num_lasers, const unsigned int scans_per_block,
00011 boost::shared_ptr<tf::TransformListener> tf_ptr)
00012 : DataContainerBase(
00013 max_range, min_range, target_frame, fixed_frame,
00014 num_lasers, 0, false, scans_per_block, tf_ptr, 5,
00015 "x", 1, sensor_msgs::PointField::FLOAT32,
00016 "y", 1, sensor_msgs::PointField::FLOAT32,
00017 "z", 1, sensor_msgs::PointField::FLOAT32,
00018 "intensity", 1, sensor_msgs::PointField::FLOAT32,
00019 "ring", 1, sensor_msgs::PointField::UINT16),
00020 iter_x(cloud, "x"), iter_y(cloud, "y"), iter_z(cloud, "z"),
00021 iter_intensity(cloud, "intensity"), iter_ring(cloud, "ring")
00022 {
00023 }
00024
00025 void OrganizedCloudXYZIR::newLine()
00026 {
00027 iter_x = iter_x + config_.init_width;
00028 iter_y = iter_y + config_.init_width;
00029 iter_z = iter_z + config_.init_width;
00030 iter_ring = iter_ring + config_.init_width;
00031 iter_intensity = iter_intensity + config_.init_width;
00032 ++cloud.height;
00033 }
00034
00035 void OrganizedCloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){
00036 DataContainerBase::setup(scan_msg);
00037 iter_x = sensor_msgs::PointCloud2Iterator<float>(cloud, "x");
00038 iter_y = sensor_msgs::PointCloud2Iterator<float>(cloud, "y");
00039 iter_z = sensor_msgs::PointCloud2Iterator<float>(cloud, "z");
00040 iter_intensity = sensor_msgs::PointCloud2Iterator<float>(cloud, "intensity");
00041 iter_ring = sensor_msgs::PointCloud2Iterator<uint16_t >(cloud, "ring");
00042 }
00043
00044
00045 void OrganizedCloudXYZIR::addPoint(float x, float y, float z,
00046 const uint16_t ring, const uint16_t , const float distance, const float intensity)
00047 {
00054 if (pointInRange(distance))
00055 {
00056 if(config_.transform)
00057 transformPoint(x, y, z);
00058
00059 *(iter_x+ring) = x;
00060 *(iter_y+ring) = y;
00061 *(iter_z+ring) = z;
00062 *(iter_intensity+ring) = intensity;
00063 *(iter_ring+ring) = ring;
00064 }
00065 else
00066 {
00067 *(iter_x+ring) = nanf("");
00068 *(iter_y+ring) = nanf("");
00069 *(iter_z+ring) = nanf("");
00070 *(iter_intensity+ring) = nanf("");
00071 *(iter_ring+ring) = ring;
00072 }
00073 }
00074 }
00075