0
$\begingroup$

Rosanswers logo

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

Originally posted by oswinium on ROS Answers with karma: 105 on 2014-03-14

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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();
    }

Originally posted by Wolf with karma: 7555 on 2014-03-14

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by oswinium on 2014-03-17:
This worked; thank you.

Comment by ROSkinect on 2014-07-17:
Can you send me the whole program if you please

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.