Changes to openni_camera needed for SXGA resolution with xtion

gistfile1.txt

diff --git a/cfg/OpenNI.cfg b/cfg/OpenNI.cfg
index 82fbd70..d023a2f 100755
--- a/cfg/OpenNI.cfg
+++ b/cfg/OpenNI.cfg
@@ -7,16 +7,18 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 
 # TODO Only offer modes supported by known hardware
-output_mode_enum = gen.enum([  gen.const(  "SXGA_15Hz", int_t, 1, "1280x1024@15Hz"),
-                               gen.const(   "VGA_30Hz", int_t, 2, "640x480@30Hz"),
-                               gen.const(   "VGA_25Hz", int_t, 3, "640x480@25Hz"),
-                               gen.const(  "QVGA_25Hz", int_t, 4, "320x240@25Hz"),
-                               gen.const(  "QVGA_30Hz", int_t, 5, "320x240@30Hz"),
-                               gen.const(  "QVGA_60Hz", int_t, 6, "320x240@60Hz"),
-                               gen.const( "QQVGA_25Hz", int_t, 7, "160x120@25Hz"),
-                               gen.const( "QQVGA_30Hz", int_t, 8, "160x120@30Hz"),
-                               gen.const( "QQVGA_60Hz", int_t, 9, "160x120@60Hz")],
-                               "output mode")
+output_mode_enum = gen.enum([
+    gen.const(  "SXGA_30Hz", int_t, 1, "1280x1024@30Hz"),
+    gen.const(  "SXGA_15Hz", int_t, 1, "1280x1024@15Hz"),
+    gen.const(   "VGA_30Hz", int_t, 2, "640x480@30Hz"),
+    gen.const(   "VGA_25Hz", int_t, 3, "640x480@25Hz"),
+    gen.const(  "QVGA_25Hz", int_t, 4, "320x240@25Hz"),
+    gen.const(  "QVGA_30Hz", int_t, 5, "320x240@30Hz"),
+    gen.const(  "QVGA_60Hz", int_t, 6, "320x240@60Hz"),
+    gen.const( "QQVGA_25Hz", int_t, 7, "160x120@25Hz"),
+    gen.const( "QQVGA_30Hz", int_t, 8, "160x120@30Hz"),
+    gen.const( "QQVGA_60Hz", int_t, 9, "160x120@60Hz")],
+                            "output mode")
 
 gen.add("image_mode", int_t, 0, "Image output mode for the color/grayscale image", 2, 1, 9, edit_method = output_mode_enum)
 gen.add("depth_mode", int_t, 0, "depth output mode", 2, 2, 9, edit_method = output_mode_enum)
diff --git a/src/nodelets/driver.cpp b/src/nodelets/driver.cpp
index c13cf28..4249ba2 100644
--- a/src/nodelets/driver.cpp
+++ b/src/nodelets/driver.cpp
@@ -42,6 +42,7 @@
 #include "openni_camera/openni_depth_image.h"
 #include "openni_camera/openni_ir_image.h"
 #include "openni_camera/openni_image_yuv_422.h"
+#include "openni_camera/openni_image_rgb24.h"
 #include <sensor_msgs/image_encodings.h>
 #include <sensor_msgs/distortion_models.h>
 #include <boost/algorithm/string/replace.hpp>
@@ -480,6 +481,12 @@ void DriverNodelet::publishRgbImage(const openni_wrapper::Image& image, ros::Tim
       downscale = true;
     }
   }
+  else if(image.getEncoding() == openni_wrapper::Image::RGB)
+  {
+      rgb_msg->encoding = sensor_msgs::image_encodings::RGB8;
+      rgb_msg->step = image_width_ * 3;
+      downscale = true;
+  }
   rgb_msg->height = image_height_;
   rgb_msg->width = image_width_;
   rgb_msg->data.resize(rgb_msg->height * rgb_msg->step);
@@ -496,6 +503,11 @@ void DriverNodelet::publishRgbImage(const openni_wrapper::Image& image, ros::Tim
       openni_wrapper::ImageYUV422 yuv_image(image.getMetaDataPtr());
       yuv_image.fillRGB(image_width_, image_height_, &rgb_msg->data[0]);
     }
+    else if (image.getEncoding() == openni_wrapper::Image::RGB)
+    {
+        openni_wrapper::ImageRGB24 rgb_image(image.getMetaDataPtr());
+        rgb_image.fillRGB(image_width_, image_height_, &rgb_msg->data[0]);
+    }
   }
   else
     image.fillRaw(&rgb_msg->data[0]);
@@ -774,6 +786,12 @@ void DriverNodelet::updateModeMaps ()
 
   output_mode.nXRes = XN_SXGA_X_RES;
   output_mode.nYRes = XN_SXGA_Y_RES;
+  output_mode.nFPS  = 30;
+  xn2config_map_[output_mode] = OpenNI_SXGA_30Hz;
+  config2xn_map_[OpenNI_SXGA_30Hz] = output_mode;
+
+  output_mode.nXRes = XN_SXGA_X_RES;
+  output_mode.nYRes = XN_SXGA_Y_RES;
   output_mode.nFPS  = 15;
   xn2config_map_[output_mode] = OpenNI_SXGA_15Hz;
   config2xn_map_[OpenNI_SXGA_15Hz] = output_mode;
diff --git a/src/openni_device_primesense.cpp b/src/openni_device_primesense.cpp
index fd00d58..2270dc3 100644
--- a/src/openni_device_primesense.cpp
+++ b/src/openni_device_primesense.cpp
@@ -35,7 +35,9 @@
  *
  */
 #include <openni_camera/openni_device_primesense.h>
-#include <openni_camera/openni_image_yuv_422.h>
+//#include <openni_camera/openni_image_yuv_422.h>
+#include <openni_camera/openni_image_bayer_grbg.h>
+#include <openni_camera/openni_image_rgb24.h>
 #include <iostream>
 #include <sstream>
 #include <boost/thread/mutex.hpp>
@@ -55,17 +57,31 @@ DevicePrimesense::DevicePrimesense (xn::Context& context, const xn::NodeInfo& de
   setImageOutputMode (getDefaultImageMode ());
   setIROutputMode (getDefaultIRMode ());
 
+  /*
   unique_lock<mutex> image_lock(image_mutex_);
   XnStatus status = image_generator_.SetIntProperty ("InputFormat", 5);
   if (status != XN_STATUS_OK)
     THROW_OPENNI_EXCEPTION ("Error setting the image input format to Uncompressed 8-bit BAYER. Reason: %s", xnGetStatusString (status));
+    */
 
-  status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_YUV422);
+
+  unique_lock<mutex> image_lock(image_mutex_);
+
+  XnStatus status;
+  /*
+            status = image_generator_.SetIntProperty ("InputFormat", 0);
+  if (status != XN_STATUS_OK)
+    THROW_OPENNI_EXCEPTION ("Error setting the image input format to compressed 8-bit BAYER. Reason: %s", xnGetStatusString (status));
+
+  //status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_YUV422);
+  status = image_generator_.SetPixelFormat (XN_PIXEL_FORMAT_RGB24);
   if (status != XN_STATUS_OK)
     THROW_OPENNI_EXCEPTION ("Failed to set image pixel format to YUV422. Reason: %s", xnGetStatusString (status));
+      */
 
   image_lock.unlock ();
 
+
   lock_guard<mutex> depth_lock(depth_mutex_);
   status = depth_generator_.SetIntProperty ("RegistrationType", 1);
   if (status != XN_STATUS_OK)
@@ -88,7 +104,7 @@ DevicePrimesense::~DevicePrimesense () throw ()
 
 bool DevicePrimesense::isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw ()
 {
-  return ImageYUV422::resizingSupported (input_width, input_height, output_width, output_height);
+  return ImageRGB24::resizingSupported (input_width, input_height, output_width, output_height);
 }
 
 //void DevicePrimesense::setImageOutputMode (const XnMapOutputMode& output_mode) throw (OpenNIException)
@@ -166,6 +182,11 @@ void DevicePrimesense::enumAvailableModes () throw (OpenNIException)
 
   // RGB Modes
   output_mode.nFPS = 30;
+  output_mode.nXRes = XN_SXGA_X_RES;
+  output_mode.nYRes = XN_SXGA_Y_RES;
+  available_image_modes_.push_back (output_mode);
+
+  output_mode.nFPS = 30;
   output_mode.nXRes = XN_VGA_X_RES;
   output_mode.nYRes = XN_VGA_Y_RES;
   available_image_modes_.push_back (output_mode);
@@ -198,7 +219,9 @@ void DevicePrimesense::enumAvailableModes () throw (OpenNIException)
 
 boost::shared_ptr<Image> DevicePrimesense::getCurrentImage (boost::shared_ptr<xn::ImageMetaData> image_data) const throw ()
 {
-  return boost::shared_ptr<Image> ( new ImageYUV422 (image_data) );
+  //return boost::shared_ptr<Image> ( new ImageYUV422 (image_data) );
+    //return boost::shared_ptr<Image> ( new ImageBayerGRBG (image_data, ImageBayerGRBG::EdgeAwareWeighted) );
+    return boost::shared_ptr<Image> ( new ImageRGB24 (image_data) );
 }
 
 void DevicePrimesense::startImageStream () throw (OpenNIException)