Low hz rate on pointcloud publisher that uses the pointcloud topic

Model: Zed2
ZED Sdk:3.7.2
GPU: NVIDIA GeForce 1660 TI

Using roslaunch zed_wrapper zed2.launch

When subscribing to the zed2 pointcloud topic and using it with PCL the published topic is usually at 1 hz, it works fine for the first few seconds and then drops heavily in freq if something is moving in view.

I’ve seen previous inquiries on this and have used those solutions, but nothing so far.
Zed.yaml has not been altered
Common.yaml only the grab_rate, point_cloud_freq and pub_frame_rate have all been altered to 60
The TF chain map → odom → base_link → zed2_base_link is present.

Hi @BakriIWNL
welcome to the Stereolabs community.

How are you measuring the frequency of the topic?

What do you obtain with
rostopic hz <topic_name>?

a lot of “no message received” followed with low average values of around 0.1-1 when using rostopic rostopic hz

Can you post the TF tree generated with rqt?

as a new user I can’t upload attachments. is there any preferred way to share it?

You can post the link to a 3rd-party download service (google drive, dropbox, etc)

1 Like

Hi @BakriIWNL
the TF tree is indeed correct, thank you for sharing.
It’s really strange that point cloud topics are published so “slowly”.
Did you change the common.yaml file in any way?

As mentioned only pub rate, grab rate and pointcloud frequency have been altered.
To avoid confusion, the low hz rate is found in the topic that publishes pcl pointclouds using the zed topic, but the zed pointcloud topic itself works at the expected frequency

Can you provide more details?

I subscribe to the zed2/zed_node/point_cloud/cloud_registered topic, which runs at the expected 60 hz
I then convert the topic from sensor_msg::pointcloud2 to pcl::pointcloud,
The publisher that publishes the pcl::pointcloud itself runs at a low hz rate, despite only just converting.

In that case, you must debug the converter node, maybe it’s performing operations in a not good way.
What node are you using?

I am sorry, but I am relatively inexperienced with ROS so I don’t quite understand what you mean by node, but I will assume you mean the part of the code that converts, which I will provide.

void callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  //convert sensor_msg::pointcloud2 to pcl::PointCloud
  pcl::PCLPointCloud2 pcl_pc2;
  pcl_conversions::toPCL(*cloud_msg,pcl_pc2);
  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);
  

  
  pcl::PointCloud<pcl::PointXYZ> cloud;

  //loop on each pointcloud
  BOOST_FOREACH (const pcl::PointXYZ &pt, temp_cloud->points)
  { 
    //testing threshold, to see if this code works or not, by limiting the range of the lidar
    pcl::PointXYZ test;
    if (pt.x >= 0.5 && pt.x <= 5 && pt.y >= -1 && pt.y <= 1 && pt.z >= -1 && pt.z <= 1)
    {
      test.x = pt.x;
      test.y = pt.y;
      test.z = pt.z;

      cloud.points.push_back(test);
    }
  }

  ROS_INFO("done");

  pcl::PointCloud<pcl::PointXYZ>::Ptr result (new pcl::PointCloud<pcl::PointXYZ>);
  result=cloud.makeShared();



  sensor_msgs::PointCloud2 geometric_filtered_cloud;
  pcl::toROSMsg(*result,geometric_filtered_cloud);
  geometric_filtered_cloud.header.frame_id="map";
  geomtrical_filter_pc_pub.publish (geometric_filtered_cloud);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "lidar_cone_detection");
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe("/zed2/zed_node/point_cloud/cloud_registered", 5, callback);
  geomtrical_filter_pc_pub = nh.advertise<sensor_msgs::PointCloud2> ("/geomtrical_filter_points", 5);
  
  ros::spin();
}

This has been tested on a simulated 128 channel lidar with no issues.

I suggest you benchmark the code of the callback. I see a few possible bottlenecks and more than a memory copy that can slow down the conversion.
Here you can find an example of very fast conversion from sl::Mat to PCL.
Converting from Pointcloud2 to PCL is very similar.

Thanks for the tip, will attempt the change the conversion once I’m back in the lab and will update on this post asap.
But if you do not mind me asking, why does this bottleneck happen? this code has been tested and works perfectly fine when using a 128 channel lidar, even if its simulated, there should be more pointclouds to convert, hence worse off performance.

How many points does the 3D lidar provide? Are the measures in floating point values or uint16 values?
What is the resolution used for the ZED ROS node?

1-2 million points per second in floating points values
And the resolution is 1344x376

OK, the lower resolution and the absence of color information can explain the difference in the performance.

Ah okay, thank you for the quick responses, will update ASAP

1 Like