Ask Your Question
0

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

asked 2017-03-08 10:42:19 -0600

ateator gravatar image

updated 2017-03-08 15:40:07 -0600

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.

edit retag flag offensive close merge delete

Comments

I am always more than happy to post more code or a deeper explanation of what I'm doing, if necessary.

ateator gravatar imageateator ( 2017-03-08 15:40:45 -0600 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2017-03-10 06:33:36 -0600

ateator gravatar image

updated 2017-03-10 06:34:29 -0600

This is what works. It now runs without crashing but the crashing was due to an unrelated issue: ROS_ERROR being used to print out information during runtime. I commented out all my ROS_ERROR's called for printing out variable information and it runs great with the code given below.

 sensor_msgs::convertPointCloudToPointCloud2 (cloud_out, cloud2_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);
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-03-08 10:42:19 -0600

Seen: 295 times

Last updated: Mar 10 '17