Code for processing point cloud is slow

asked 2020-03-26 10:35:55 -0500

christophecricket gravatar image

updated 2020-03-27 04:08:12 -0500

Hi All

I have a follow up to my question that I asked over here : https://answers.ros.org/question/3473...

For context, I'm writing a piece of code that takes in a point cloud from a kinect2 using iai kinect (https://github.com/code-iai/iai_kinect2), and a keyboard input, using this guy's package (https://github.com/lrse/ros-keyboard). The keyboard moves through the environment based on the keyboard input, and the points in the cloud exert a repulsive force on it if it gets too close.

I expect the speed of my program for this too be mostly static, as it contains a while loop in which I spin my callbacks and iterate over the point cloud to compute force, and then some package of ROS messages, and that's it. However, I observe a gradual decrease in speed as I'm running my code (it'll start at like 10Hz, and then next loop it will have been 5 Hz, then 2 Hz, then 0.5 Hz, etc...). This is quite unexpected, and I have no idea why this would be.

Some things to note: as in my previous post, the loop seems to be hanging at the spinOnce() statement, and I have compiler optimisations turned on. The code is still slow if I remove all the ROS interfaces other than the one subscribing to the point cloud. Below is my code.

EDIT: I also removed the loop_rate() from my code and that didn't seem to have an effect.

//containers for point clouds at different processing stages
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr outputCloudFilt(new pcl::PointCloud<pcl::PointXYZ>);

std::vector<int> indices;

// Create the filtering object
pcl::VoxelGrid<pcl::PointXYZ> sor;

//the point interacting with the cloud
std::vector<double> HIP = {0,0,0};

//operating frequency
double fc;

//current distance from HIP to a given point in the cloud
double current_point_distance;

//vector from HIP toa given point in the cloud
std::vector<double> current_Pkxi = {0,0,0};

//output interaction force
std::vector<double> force_render = {0,0,0};

//output types
visualization_msgs::Marker point_message;
geometry_msgs::WrenchStamped force_message;

//forward declare callbacks
void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg);
void keyCallback(const keyboard::KeyConstPtr& msg);
double norm3(std::vector<double> in);

int main(int argc, char** argv){

  // Initialize ROS
  ros::init(argc, argv, "pclistener");
  ros::NodeHandle n;
  ros::Rate loop_rate(50);
  ros::Time previous_loop_entry = ros::Time::now();
  ros::Time current_loop_entry = ros::Time::now();

  ros::Subscriber key_sub = n.subscribe("/keyboard/keyup", 1000, keyCallback);
  ros::Subscriber pc_sub = n.subscribe("/kinect2/qhd/points", 1000, cloudCallback);
  ros::Publisher HIP_pub = n.advertise<visualization_msgs::Marker>("/pcbmmt/HIP",1000);
  ros::Publisher force_pub = n.advertise<geometry_msgs::WrenchStamped>("/pcbmmt/force",1000);


  while(ros::ok()){

    //reset output force
    force_render = {0,0,0};

    //compute iteration frequency when it's below 50Hz
    current_loop_entry = ros::Time::now();
    fc = 1/(current_loop_entry-previous_loop_entry).toSec();
    previous_loop_entry = current_loop_entry;

    std::cout << fc << std::endl;
    std::cout << outputCloudFilt->points.size() << std::endl;

    ros::spinOnce();

    //protect against the point cloud ...
(more)
edit retag flag offensive close merge delete

Comments

2

I'm leaving it formatted like shit because what the parser does and doesn't interpret as code seems to be decided by a coin flip or something

no, that's caused by you not formatting it properly.

Could I please ask you to watch your language?

In any case: in the future: paste your code into your question, select the lines and press ctrl+k or the Preformatted Text button (ie: the one with 101010 on it).

gvdhoorn gravatar image gvdhoorn  ( 2020-03-26 11:44:40 -0500 )edit

Done. Sorry. That I'm frustrated by my problem is no one else's fault but mine.

christophecricket gravatar image christophecricket  ( 2020-03-26 12:07:17 -0500 )edit

As a quick comment: pub-sub with PointCloud2 with regular nodes is going to always be difficult. PointCloud2s can just be really big, and seeing as you're subscribing to the qhd topic, they probably are. And with large I mean: several 100MB/sec, per topic.

This potentially leads to situations where the deserialisation of the message itself already takes longer than you're willing to wait.

To see whether (de)serialisation is actually the issue, you could do two things (I'd actually do both):

  1. subscribe to the sd topic and see whether things improve, and
  2. rewrite your node as a nodelet, and load it into the manager which the iai-kinect2 driver starts

Note that nodelets come with their own set of disadvantages, but it's always a trade-off between performance and other *-bilities in these cases.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-27 08:58:41 -0500 )edit