Ask Your Question
0

Publishing data using OpenNI

asked 2011-07-07 06:02:19 -0500

lakshmen gravatar image

updated 2016-10-24 09:03:07 -0500

ngrennan gravatar image

Hi guys,

Although, i am able to find the depth pointer and find the depth value. Now i need to publish the data and want to visualize the point cloud in Rviz. I have written a code and would like to have your opinions.

    /* OpenCV Includes */
#include "opencv2\opencv.hpp"
#include "opencv2\highgui\highgui.hpp"
#include "opencv2\imgproc\imgproc.hpp"

/* OpenNI Includes */
#include <XnCppWrapper.h>
#include <XnOpenNI.h>
#include <XnLog.h>
#include <XnOS.h>

/* PCL Includes */
#include "pcl/ModelCoefficients.h"
#include "pcl/console/parse.h"
#include "pcl/console/print.h"
#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include <pcl/features/cvfh.h>
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include "pcl/filters/extract_indices.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/statistical_outlier_removal.h"
#include "pcl/kdtree/kdtree.h"
#include "pcl/sample_consensus/method_types.h"
#include "pcl/sample_consensus/model_types.h"
#include "pcl/segmentation/sac_segmentation.h"
#include "pcl/segmentation/extract_clusters.h"
#include <boost/filesystem.hpp>

/* ROS Includes */
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

#define SAMPLE_XML_PATH "C:/Program Files (x86)/OpenNI/Data/SamplesConfig.xml"
#define X_RES 640
#define Y_RES 480
#define MAX_DEPTH 10000

using namespace std;
using namespace xn;
using namespace pcl::console;

/* Globals */
Context         context;
ImageGenerator      g_image;
DepthGenerator      d_image;
ImageMetaData       g_imageMD;
EnumerationErrors   errors;
XnStatus        nRetVal = XN_STATUS_OK;
unsigned short      depth[MAX_DEPTH];
char *depth_data;

/* Typedefs */
typedef pcl::PointCloud<pcl::PointXYZRGBNormal> PointCloud;

void raw2depth(){
  for (int i=0; i<MAX_DEPTH; i++) {
  float v = (float)i/MAX_DEPTH;//for visualization purposes only
  v = powf(v, 2);
  v = v*36*256;
  depth[i] = v;
  }
}

void depth2rgb(const XnDepthPixel* Xn_disparity){
  for (int i=0; i<307200; i++) {
    int pval = depth[Xn_disparity[i]];
    int lb = pval & 0xff;
    switch (pval>>8) {
      case 0:
        depth_data[3*i+0] = 255;
        depth_data[3*i+1] = 255;
        depth_data[3*i+2] = 255-lb;
        break;
      case 1:
        depth_data[3*i+0] = 255;
        depth_data[3*i+1] = lb;
        depth_data[3*i+2] = 0;
        break;
      case 2:
        depth_data[3*i+0] = 255-lb;
        depth_data[3*i+1] = 255;
        depth_data[3*i+2] = 0;
        break;
      case 3:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 255;
        depth_data[3*i+2] = lb;
        break;
      case 4:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 255-lb;
        depth_data[3*i+2] = 255;
        break;
      case 5:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 0;
        depth_data[3*i+2] = 255-lb;
        break;
      default:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 0;
        depth_data[3*i+2] = 0;
        break;
    }
  }
}


void rgbdInit(){
  //Initialize context object
  nRetVal = context.Init();
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to initialize context: %s\n", xnGetStatusString(nRetVal));
  }

  // Create a DepthGenerator node
  nRetVal = d_image.Create(context);
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to create depth generator: %s\n", xnGetStatusString(nRetVal));
  }

  // Create a ImageGenerator node
  nRetVal = g_image.Create(context);
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to create image generator: %s\n", xnGetStatusString(nRetVal));
  }

  // Make it start generating data
  nRetVal = context.StartGeneratingAll();
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed generating data: %s\n", xnGetStatusString(nRetVal));
  }

/*  //Sync the DepthGenerator with the ImageGenerator
  nRetVal = d_image.GetFrameSyncCap().FrameSyncWith(g_image);
  if (nRetVal ...
(more)
edit retag flag offensive close merge delete

Comments

What do you expect this to do? What doesn't work? Do you really expect us to compile and debug your code in order to find out the answer to these two questions ourselves? Also, stating that you need an answer urgently doesn't help getting an answer faster, it just makes you sound impatient.
Martin Günther gravatar image Martin Günther  ( 2011-07-10 23:18:01 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2011-07-07 07:42:17 -0500

dornhege gravatar image

You have two main loops.

Only when the first one quits, the second one runs. You should merge them.

edit flag offensive delete link more

Comments

is it better now?
lakshmen gravatar image lakshmen  ( 2011-07-07 12:22:22 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2011-07-07 06:02:19 -0500

Seen: 961 times

Last updated: Jul 07 '11