Put sensor_msgs::PointCloud into row of sensor_msgs::PointCloud2

edit

Hey all,

I am currently trying to create a function that will take a sensor_msgs::PointCloud that has 953 points in it and replace a row of a sensor_msgs::PointCloud2 that has height 36 and width 953 (we don't know if this cloud is full yet). I would like to have access of this PointCloud2 outside of my function but I just don't have the skill to do this. Here is what I have so far:

void replaceRowInPointCloud2(sensor_msgs::PointCloud2 cloud2_out, sensor_msgs::PointCloud cloud_in, int row){

  for (int i=0; i<954; i++){
    int arrayPosition = row*cloud2_out.row_step + i*cloud2_out.point_step;
    int arrayPosX = arrayPosition + cloud2_out.fields[0].offset; // X has an offset of 0
    int arrayPosY = arrayPosition + cloud2_out.fields[1].offset; // Y has an offset of 4
    int arrayPosZ = arrayPosition + cloud2_out.fields[2].offset; // Z has an offset of 8

    memcpy(&cloud2_out.data[arrayPosX],&cloud_in.points[i].x , sizeof(float));
    memcpy(&cloud2_out.data[arrayPosY],&cloud_in.points[i].y , sizeof(float));
    memcpy(&cloud2_out.data[arrayPosZ],&cloud_in.points[i].z , sizeof(float));
  }

}

This code compiles but crashes immediately on first call. I would appreciate any help, tips, or suggestions. Thank you!

EDIT 1:

This is what I've done that compiles and runs, but never actually publishes a cloud that Rviz can view. cloud_b is a global variable so that I can just change the point values, convert to sensor_msgs, then publish at any time. I think that the ROS messages don't come across because they are invalid, but I dont know why. It's probably some sort of formatting issue.

pcl::fromROSMsg(cloud2_out,cloud_a);
for (int i=0;i<594;i++){
  cloud_b.points[row*953+i]= cloud_a.points[i];
}
pcl::toROSMsg(cloud_b,cloud2_out);

EDIT 2:

This is what I have now..

    sensor_msgs::convertPointCloudToPointCloud2 (cloud_out, cloud2_out);
    cloud_pub.publish(cloud_out);
   pcl::fromROSMsg(cloud2_out,cloud_a);
    for (int i=0;i<953;i++)
    {
      cloud_b.points[posindex/5*953+i]=cloud_a.points[i];
    }
    pcl::toROSMsg(cloud_b,cloud2_out);
    cloud2_pub.publish(cloud2_out);

Which compiles and runs. The first time it runs, it will run for quite a long time before crashing. I suspect it crashes due to memory leaks. When I use rostopic echo on my topic, it shows a valid stream of numbers with no repeating sequences. After it crashes once, further launches of the program will run 8 runs of my callback. If i rostopic echo my topic, it will display a stream of points (many repeated sequences of points, with many zero's) from my PointCload2, and it shows as dense. When the program does not crash immediately, rviz shows no warnings. When it does crash immediately, the terminal with Rviz running also shows "Invalid argument passed to canTransform argument source_fram in tf2 frame_ids cannot be empty". How do I fix this? I can give any explanation as needed and again I appreciate any help.