Rviz visualizes large data point clouds

asked 2020-05-13 05:28:37 -0500

3fmeow gravatar image

I tried to distribute point clouds in c++ and visualize them in RViz. The number of point clouds will increase over time. Below are the details of the environment

  • OS: windows10
  • CPU: intel i5-9400f
  • GPU: nvidia GTX 1660 Ti
  • Memory: 16G

Below is the code snippet

class RecvLidarData{

private:

    ros::NodeHandle nh;  
    ros::Subscriber subLaserCloud;
    ros::Publisher pubLaserCloud;
    std_msgs::Header cloudHeader;
    pcl::PointCloud<PointType>::Ptr laserCloudIn;
    pcl::PointCloud<PointType>::Ptr laserCloudOut;

public:    

    RecvLidarData():     nh("~")
    {
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> ("/topic0", 1000, &RecvLidarData::cloudHandler, this);
        pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2> ("/topic1", 1); 
        allocMemory();
    }

    ~RecvLidarData(){};

    void allocMemory()
    {
        laserCloudIn.reset(new pcl::PointCloud<PointType>());
        laserCloudIn->clear();
        laserCloudOut.reset(new pcl::PointCloud<PointType>());  
        laserCloudOut->clear();
    }
    void resetParameters()
    {
        laserCloudIn->clear();
    }

    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {    
        cloudHeader = laserCloudMsg->header;
        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);

        *laserCloudOut += *laserCloudIn;
    }

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
    {
        copyPointCloud(laserCloudMsg);
        publishCloud();
        resetParameters();
    }

    void publishCloud()
    {              
        sensor_msgs::PointCloud2 laserCloudTemp;
        pcl::toROSMsg(*laserCloudOut, laserCloudTemp);       
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";        
        pubLaserCloud.publish(laserCloudTemp);
    }    
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "test");
    RecvLidarData IP;    
    ROS_INFO("\033[1;32m---->\033[0m test Started.");          
    ros::spin();
    return 0;
}

Topic0 publishes fixed-size point cloud data at 10hz. I monitored the rate at which messages are being published using rostopic hz topic1 and found the following info

average rate: 8.202
min: 0.023s max: 0.233s std dev: 0.05518s window: 9
average rate: 5.777
min: 0.023s max: 0.334s std dev: 0.08966s window: 13
average rate: 4.785
min: 0.023s max: 0.467s std dev: 0.12194s window: 15
...
average rate: 0.130
min: 0.023s max: 38.005s std dev: 10.60088s window: 42

And the command rostopic bw topic1 is returning the following info

average: 4.05MB/s
mean: 2.38MB min: 0.16MB max: 6.72MB window: 13
average: 3.58MB/s
mean: 2.38MB min: 0.16MB max: 6.72MB window: 13
average: 4.06MB/s
mean: 2.80MB min: 0.16MB max: 8.19MB window: 14
...
average: 4.67MB/s
mean: 53.97MB min: 0.16MB max: 228.72MB window: 35
average: 4.66MB/s
mean: 53.97MB min: 0.16MB max: 228.72MB window: 35

The frame rate in RViz drops to 0fps when the point cloud size approaches 10 million points. I found that the most time-consuming was the following two lines, which took up almost all the execution time of the callback function

pcl::toROSMsg(*laserCloudOut, laserCloudTemp); 
pubLaserCloud.publish(laserCloudTemp);

Is there any way to speed up the publisher or rviz visualization rate? Thanks in advance.

edit retag flag offensive close merge delete