Publishing data using OpenNI

edit

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