Code for processing point cloud is slow

edit

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 ...