Skip to content

Instantly share code, notes, and snippets.

@jonbinney
Created January 11, 2013 05:31
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
Save jonbinney/4508201 to your computer and use it in GitHub Desktop.
Changes to openni_camera needed for SXGA resolution with xtion
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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment