add fallback method that uses depth on color

openni2_camera/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(openni2_camera)
 
-find_package(catkin REQUIRED camera_info_manager dynamic_reconfigure image_transport  nodelet sensor_msgs roscpp message_generation)
+find_package(catkin REQUIRED camera_info_manager dynamic_reconfigure image_transport  nodelet sensor_msgs std_srvs roscpp message_generation)
 
 find_package(Boost REQUIRED COMPONENTS system thread)
 
@@ -20,7 +20,7 @@ generate_messages()
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES openni2_wrapper
-  CATKIN_DEPENDS camera_info_manager dynamic_reconfigure image_transport nodelet sensor_msgs roscpp message_runtime
+  CATKIN_DEPENDS camera_info_manager dynamic_reconfigure image_transport nodelet sensor_msgs std_srvs roscpp message_runtime
   DEPENDS libopenni2
 )
 
@@ -89,6 +89,3 @@ install(DIRECTORY include/${PROJECT_NAME}/
 install(FILES openni2_nodelets.xml
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
-
-
-

openni2_camera/include/openni2_camera/openni2_driver.h

@@ -38,6 +38,7 @@
 #include <boost/function.hpp>
 
 #include <sensor_msgs/Image.h>
+#include <std_srvs/SetBool.h>
 
 #include <dynamic_reconfigure/server.h>
 #include <openni2_camera/OpenNI2Config.h>
@@ -92,6 +93,7 @@ class OpenNI2Driver
   void irConnectCb();
 
   bool getSerialCb(openni2_camera::GetSerialRequest& req, openni2_camera::GetSerialResponse& res);
+  bool switchNoColorFallback(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res);
 
   void configCb(Config &config, uint32_t level);
 
@@ -101,6 +103,7 @@ class OpenNI2Driver
   int lookupVideoModeFromDynConfig(int mode_nr, OpenNI2VideoMode& video_mode);
 
   sensor_msgs::ImageConstPtr rawToFloatingPointConversion(sensor_msgs::ImageConstPtr raw_image);
+  sensor_msgs::ImageConstPtr rawToGrayConversion(sensor_msgs::ImageConstPtr raw_image);
 
   void setIRVideoMode(const OpenNI2VideoMode& ir_video_mode);
   void setColorVideoMode(const OpenNI2VideoMode& color_video_mode);
@@ -132,9 +135,14 @@ class OpenNI2Driver
    */
   bool serialnumber_as_name_;
 
+  bool no_color_fallback_;
+
   /** \brief get_serial server*/
   ros::ServiceServer get_serial_server;
 
+/** \brief get_serial server*/
+  ros::ServiceServer switch_no_color_fallback_server;
+
   /** \brief reconfigure server*/
   boost::shared_ptr<ReconfigureServer> reconfigure_server_;
   bool config_init_;

openni2_camera/package.xml

@@ -17,6 +17,7 @@
   <build_depend>camera_info_manager</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>image_transport</build_depend>
@@ -27,6 +28,7 @@
   <run_depend>nodelet</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_srvs</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>message_runtime</run_depend>

openni2_camera/src/openni2_driver.cpp

@@ -55,7 +55,8 @@ OpenNI2Driver::OpenNI2Driver(ros::NodeHandle& n, ros::NodeHandle& pnh) :
     depth_subscribers_(false),
     depth_raw_subscribers_(false),
     enable_reconnect_(false),
-    serialnumber_as_name_(false)
+    serialnumber_as_name_(false),
+    no_color_fallback_(false)
 {
 
   genVideoModeTableMap();
@@ -162,13 +163,20 @@ void OpenNI2Driver::advertiseROSTopics()
 
   get_serial_server = nh_.advertiseService("get_serial", &OpenNI2Driver::getSerialCb,this);
 
+  switch_no_color_fallback_server = nh_.advertiseService("switch_no_color_fallback", &OpenNI2Driver::switchNoColorFallback, this);
 }
 
 bool OpenNI2Driver::getSerialCb(openni2_camera::GetSerialRequest& req, openni2_camera::GetSerialResponse& res) {
   res.serial = device_manager_->getSerial(device_->getUri());
   return true;
 }
 
+bool OpenNI2Driver::switchNoColorFallback(std_srvs::SetBoolRequest& req, std_srvs::SetBoolResponse& res) {
+  no_color_fallback_ = req.data;
+  res.success = true;
+  return true;
+}
+
 void OpenNI2Driver::configCb(Config &config, uint32_t level)
 {
   bool stream_reset = false;
@@ -519,6 +527,7 @@ void OpenNI2Driver::newColorFrameCallback(sensor_msgs::ImagePtr image)
       pub_color_.publish(image, getColorCameraInfo(image->width, image->height, image->header.stamp));
     }
   }
+  no_color_fallback_ = false;
 }
 
 void OpenNI2Driver::newDepthFrameCallback(sensor_msgs::ImagePtr image)
@@ -554,6 +563,11 @@ void OpenNI2Driver::newDepthFrameCallback(sensor_msgs::ImagePtr image)
       {
         image->header.frame_id = color_frame_id_;
         cam_info = getColorCameraInfo(image->width,image->height, image->header.stamp);
+        if (no_color_fallback_)
+        {
+          sensor_msgs::ImageConstPtr depth_to_gray_image = rawToGrayConversion(image);
+          pub_color_.publish(depth_to_gray_image, getColorCameraInfo(image->width, image->height, image->header.stamp));
+        }
       } else
       {
         image->header.frame_id = depth_frame_id_;
@@ -1149,4 +1163,38 @@ sensor_msgs::ImageConstPtr OpenNI2Driver::rawToFloatingPointConversion(sensor_ms
   return new_image;
 }
 
+sensor_msgs::ImageConstPtr OpenNI2Driver::rawToGrayConversion(sensor_msgs::ImageConstPtr raw_image)
+{
+  static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+  sensor_msgs::ImagePtr new_image = boost::make_shared<sensor_msgs::Image>();
+
+  new_image->header = raw_image->header;
+  new_image->width = raw_image->width;
+  new_image->height = raw_image->height;
+  new_image->is_bigendian = 0;
+  new_image->encoding = sensor_msgs::image_encodings::RGB8;
+  new_image->step = 3*sizeof(uint8_t)*raw_image->width;
+
+  std::size_t data_size = new_image->width*new_image->height;
+  new_image->data.resize(data_size*3*sizeof(uint8_t));
+
+  const unsigned short* in_ptr = reinterpret_cast<const unsigned short*>(&raw_image->data[0]);
+  uint8_t* out_ptr = reinterpret_cast<uint8_t*>(&new_image->data[0]);
+
+  for (std::size_t i = 0; i<data_size; ++i, ++in_ptr)
+  {
+    uint8_t gray_value = static_cast<uint8_t>(*in_ptr/2);
+    //r
+    *out_ptr = gray_value;
+    ++out_ptr;
+    //g
+    *out_ptr = gray_value;
+    ++out_ptr;
+    //b
+    *out_ptr = gray_value;
+    ++out_ptr;
+  }
+  return new_image;
+}
 }