From 1244b2b06e534e1035f08836c39e63216be7bde9 Mon Sep 17 00:00:00 2001 From: Ruben SMITS Date: Wed, 15 Dec 2021 17:45:25 +0100 Subject: [PATCH] add fallback method that uses depth on color --- openni2_camera/CMakeLists.txt | 7 +-- .../include/openni2_camera/openni2_driver.h | 8 +++ openni2_camera/package.xml | 2 + openni2_camera/src/openni2_driver.cpp | 50 ++++++++++++++++++- 4 files changed, 61 insertions(+), 6 deletions(-) diff --git a/openni2_camera/CMakeLists.txt b/openni2_camera/CMakeLists.txt index cc836f0..41143f0 100644 --- a/openni2_camera/CMakeLists.txt +++ b/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} ) - - - diff --git a/openni2_camera/include/openni2_camera/openni2_driver.h b/openni2_camera/include/openni2_camera/openni2_driver.h index 0d10c02..d2969d3 100644 --- a/openni2_camera/include/openni2_camera/openni2_driver.h +++ b/openni2_camera/include/openni2_camera/openni2_driver.h @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -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 reconfigure_server_; bool config_init_; diff --git a/openni2_camera/package.xml b/openni2_camera/package.xml index f307bd6..121fd10 100644 --- a/openni2_camera/package.xml +++ b/openni2_camera/package.xml @@ -17,6 +17,7 @@ camera_info_manager nodelet sensor_msgs + std_srvs roscpp dynamic_reconfigure image_transport @@ -27,6 +28,7 @@ nodelet dynamic_reconfigure sensor_msgs + std_srvs roscpp image_transport message_runtime diff --git a/openni2_camera/src/openni2_driver.cpp b/openni2_camera/src/openni2_driver.cpp index c6e24e4..3356d0a 100644 --- a/openni2_camera/src/openni2_driver.cpp +++ b/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,6 +163,7 @@ 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) { @@ -169,6 +171,12 @@ bool OpenNI2Driver::getSerialCb(openni2_camera::GetSerialRequest& req, openni2_c 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::quiet_NaN (); + + sensor_msgs::ImagePtr new_image = boost::make_shared(); + + 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(&raw_image->data[0]); + uint8_t* out_ptr = reinterpret_cast(&new_image->data[0]); + + for (std::size_t i = 0; i(*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; +} }