Building msg in c++

edit

I am trying to construct a message in c++. I am using Ros kinetic, ubuntu 16.04. I have a message that consists of a list of the Point message from geometry_msgs. The message looks as follows:

geometry_msgs/Point[] points

I use this message to transport a list of xy coordinates. The next node uses these coordinates to filter a distance from a pointcloud msg. The part were it goes wrong is when I try to construct the message that the second node publishes. This has to be a list, similair to the one that is sent from the first node. However, this should consist of xyz coordinates. The code I am using (if more is desired I'll add it):

void pixelTo3DPoint(const sensor_msgs::PointCloud2 pCloud, coord_proces::xyz xy_coord)
{
    // get width and height of 2D point cloud data
    int width = pCloud.width;
    int height = pCloud.height;
    int step;
    int n = 0;
    float min_x = 0;
    float min_y = 0;
    float min_z = 0;

    int list_size = xy_coord.points.size();

    float z_List[list_size];
    float y_List[list_size];
    float x_List[list_size];     

    coord_proces::xyz obj_coords;
    geometry_msgs::Point pt;


    for (int i = 0; i < list_size; i++)
    { 
        const int u = static_cast<int>(xy_coord.points[i].x);
        const int v = static_cast<int>(xy_coord.points[i].y);

        // Convert from u (column / width), v (row/height) to position in array
        // where X,Y,Z data starts
        int arrayPosition = v*pCloud.row_step + u*pCloud.point_step;

        // compute position in array where x,y,z data start      
        int arrayPosX = arrayPosition + pCloud.fields[0].offset; // X has an offset of 0
        int arrayPosY = arrayPosition + pCloud.fields[1].offset; // Y has an offset of 4
        int arrayPosZ = arrayPosition + pCloud.fields[2].offset; // Z has an offset of 8

        float X = 0.0;
        float Y = 0.0;
        float Z = 0.0;

        // Copy the data directly from the location in the message
        memcpy(&X, &pCloud.data[arrayPosX], sizeof(float));
        memcpy(&Y, &pCloud.data[arrayPosY], sizeof(float));
        memcpy(&Z, &pCloud.data[arrayPosZ], sizeof(float));

        if(not(isnan(X)) & not(isnan(Y)) & not(isnan(Z)))
        {
            if((0.05 < Z) & (Z < 2.0))
            {
                ROS_INFO("check1");
                ROS_INFO_STREAM(X);
                pt.x = X;
                pt.y = Y;
                pt.z = Z;
            }
        } 

        obj_coords.points[i].insert(0, pt);   
    }

    // select lowest Z
    ROS_INFO("check3");
    point_pub.publish(obj_coords);  
}

The program progresses to check2, but does not reach check3 after constructing the message. I know for a fact that the values in pt are correct. However it won't store them in obj_coords.points. I am guessing this is caused by my attempt to itterate through the list adding points until all relevant xy combinations are combined with their appropriate z.

How would I go about properly storing all coordinates in the message?