Cutting off a pointcloud2 at a certain range

edit

I am working on a mapping problem with a laser scanner. I want to cut off the 20% of points that are furthest away from the robot. So this distance should be variable for each time steps. Depending on how close we are to walls in certain areas. I came up with the following approach:

void PcFilter::pcCallback(const sensor_msgs::PointCloud2ConstPtr &pointcloud_in)
{
    unsigned int num_points = pointcloud_in->width;
    std::vector<float> ranges(num_points);
    sensor_msgs::PointCloud2 pointcloud_out;
    int cut_off_idx = int(num_points*0.8);
    pointcloud_out.header = pointcloud_in->header;
    pointcloud_out.fields = pointcloud_in->fields;
    pointcloud_out.height = pointcloud_in->height;
    pointcloud_out.width = cut_off_idx;//+1
    pointcloud_out.point_step = pointcloud_in->point_step;
    pointcloud_out.row_step = pointcloud_out.width*pointcloud_out.point_step;
    pointcloud_out.is_bigendian = pointcloud_in->is_bigendian;
    pointcloud_out.is_dense = pointcloud_in->is_dense;
    sensor_msgs::PointCloud2ConstIterator<float> iter_x(*pointcloud_in, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(*pointcloud_in, "y");
    int count = 0;
    //iterate over pointcloud an save the distance from the points in the xy plane
    for(;iter_x != iter_x.end();
        ++iter_x, ++iter_y)
    {
        ranges[count] = hypot(*iter_x, *iter_y);
        count++;
    }
    std::sort(std::begin(ranges), std::end(ranges));
    float cut_off_dist = ranges[cut_off_idx];
    float point_dist;
    sensor_msgs::PointCloud2ConstIterator<float> iter_xx(*pointcloud_in, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_yy(*pointcloud_in, "y");
    //iterate over pointcloud again and fill data in new pointcloud
    for(;iter_xx != iter_xx.end(); ++iter_xx, ++iter_yy)
    {
        point_dist = hypot(*iter_x, *iter_y);
        if (point_dist <= cut_off_dist)
        {

            //todo fill in data
            //pointcloud_out.data  =0;
        }
    }
    pub_pc2_.publish(pointcloud_out);
    }

What I cant seem to accomplish is actually filling the data field with the corresponding points. I saw sth about memcpy, but I could find an extensive tutorial. Appreciate your help!