Speedup publisher having huge amount of data
I am trying to publish point cloud and visualizing inside RViz in C++. There are approximately 8,000,000 points in the point cloud.
Below are the details of the environment-
- Intel® Xeon(R) CPU E5-2640 v2 @ 2.00GHz × 17
- GeForce GTX 1050 Ti/PCIe/SSE2
- 32 GB Memory
- Ubuntu 14.04 LTS 64 Bit OS
I noticed that publishing this point cloud at 5 FPS is taking 100 ms approximately. I monitored the rate at which messages are being published using rostopic hz topic
and found the following info-
average rate: 0.445
min: 0.595s max: 4.001s std dev: 0.82609s window: 113
After changing publishing frequency to 1 FPS, below is output returned by rostopic hz topic
-
average rate: 0.552
min: 0.200s max: 134.727s std dev: 11.34498s window: 204
Now, the command rostopic bw topic
is returning the following info-
average: 230.46MB/s
mean: 199.07MB min: 199.07MB max: 199.07MB window: 3
Later, I used nodelet and got little better results. rostopic hz topic
returns the following-
average rate: 2.481
min: 0.265s max: 0.608s std dev: 0.03269s window: 630
rostopic bw topic
is returning the following info-
average: 494.49MB/s
mean: 199.07MB min: 199.07MB max: 199.07MB window: 100
Below is the code snippet-
ros::Publisher pub_output = nh.advertise<sensor_msgs::PointCloud2>("points", 1);
// number of points in point cloud
int n = 8000000;
// create a point cloud with random data
pcl::PointCloud<pcl::PointXYZRGB> cloud;
cloud.width = n;
cloud.height = 1;
cloud.is_dense = false;
cloud.points.resize(cloud.width* cloud.height);
for (size_t i = 0; i < cloud.points.size(); ++i) {
cloud.points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud.points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud.points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
cloud.points[i].r = 255.0 * rand();
cloud.points[i].g = 255.0 * rand();
cloud.points[i].b = 255.0 * rand();
}
// convert to PointCloud2
sensor_msgs::PointCloud2 temp_cloud;
pcl::toROSMsg(cloud, temp_cloud);
// attach a frame
temp_cloud.header.frame_id = "base";
boost::shared_ptr<sensor_msgs::PointCloud2> cloud2 = boost::make_shared<sensor_msgs::PointCloud2>(temp_cloud);
ros::Rate loop_rate(5);
while (ros::ok()) {
// update the timestamp
cloud2->header.stamp = ros::Time::now();
pub_output.publish(cloud2);
ros::spinOnce();
loop_rate.sleep();
}
It clearly indicates that despite having enough resources, ROS is not able to publish the huge amount of data at high speed.
Is there any way to speed up the publisher while simultaneously visualizing it inside RViz. Please note that the publisher and RViz are in the same machine.