Subscriber not executing callback function to receive point cloud data from kinect
I have written a code to subscribe to the /camera/depth_registered/points topic to receive point cloud data from the kinect and then display it on a pcl::visualization::PCLVisualizer viewer.
The problem is that, while successfully subscribed to the topic (as proven using the rostopic info command), the callback function is not actually being executed. I know this because, at the very beginning of the function, I've added a simple print statement confirming, "Entering callback." This statement is never printed. In fact, the while loop (see below) is never exited i.e. the boolean new_cloud_available_flag is never set to true.
Here is the relevant section of the code:
//Including all possible libraries
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <ros/ros.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/people/ground_based_people_detection_app.h>
#include <pcl/common/time.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
// PCL viewer //
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
// Mutex: //
boost::mutex cloud_mutex;
bool new_cloud_available_flag;
enum { COLS = 640, ROWS = 480 };
PointCloudT::Ptr cloud(new PointCloudT);
void callback(const sensor_msgs::PointCloud2ConstPtr& msg){
std::cout << "Entering callback";
cloud_mutex.lock();
sensor_msgs::PointCloud2 msg0 = *msg; //Changing pointer to actual cloud object
PointCloudT cloud0;
pcl::fromROSMsg(msg0, cloud0); //Converting sensor_msgs::PointCloud2 to PointCloudT
*cloud = cloud0;
new_cloud_available_flag = true;
std::cout << "Converted" << endl;
cloud_mutex.unlock();
}
int main (int argc, char** argv)
{
ros::init(argc, argv, "subscriber_node");
ros::NodeHandle n;
new_cloud_available_flag = false;
ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1000, callback);
// Subscribe to kinect:
std::cout<<"initiate reading\n";
ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 50, callback);
// Wait for the first frame:
while(!new_cloud_available_flag)
{
ros::Time t0 = ros::Time::now();
while(ros::Time::now()-t0 < ros::Duration(0.001));
std::cout<<"waiting for first frame\n";
}
std::cout<<"got first frame\n";
//More code later...