ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Subscriber not executing callback function to receive point cloud data from kinect

asked 2014-03-14 05:18:42 -0500

oswinium gravatar image

updated 2016-10-24 09:10:58 -0500

ngrennan gravatar image

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...
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2014-03-14 06:46:02 -0500

Wolf gravatar image

Your callbacks are called in ros::spin(), ros::spinOnce() or via asych spinner etc. Your code in contrast just does busy waiting. Modify your wait for first frame like:

 // Wait for the first frame:
  while(!new_cloud_available_flag) 
    {
      std::cout<<"waiting for first frame\n";
      ros::Duration( 0.01 ).sleep();
      ros::spinOnce();
    }
edit flag offensive delete link more

Comments

This worked; thank you.

oswinium gravatar image oswinium  ( 2014-03-17 08:14:26 -0500 )edit

Can you send me the whole program if you please

ROSkinect gravatar image ROSkinect  ( 2014-07-17 04:55:27 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2014-03-14 05:18:42 -0500

Seen: 1,751 times

Last updated: Mar 14 '14