From 5faa464379f4ae5f8cef0ddb3340b9d24d85a869 Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Wed, 14 Mar 2018 09:14:48 +0100 Subject: [PATCH 1/2] Added support for kinect 2 --- src/libs/comm_cpp/src/ros/translators.cpp | 24 ++++++---- src/tools/rgbdViewer/CMakeLists.txt | 1 + src/tools/rgbdViewer/rgbdViewer.cpp | 2 +- src/tools/rgbdViewer/rgbdViewerKinect2.yml | 52 ++++++++++++++++++++++ 4 files changed, 70 insertions(+), 9 deletions(-) create mode 100644 src/tools/rgbdViewer/rgbdViewerKinect2.yml diff --git a/src/libs/comm_cpp/src/ros/translators.cpp b/src/libs/comm_cpp/src/ros/translators.cpp index af1f99f5c..795c9ea8a 100644 --- a/src/libs/comm_cpp/src/ros/translators.cpp +++ b/src/libs/comm_cpp/src/ros/translators.cpp @@ -8,13 +8,20 @@ namespace Comm { void - depthToRGB(const cv::Mat& float_img, cv::Mat& rgb_img){ + depthToRGB(const cv::Mat& float_img, cv::Mat& rgb_img, std::string type ){ //Process images - cv::Mat mono8_img = cv::Mat(float_img.size(), CV_8UC1); - if(rgb_img.rows != float_img.rows || rgb_img.cols != float_img.cols){ - rgb_img = cv::Mat(float_img.size(), CV_8UC3); - } - cv::convertScaleAbs(float_img, mono8_img, 255/MAXRANGEIMGD, 0.0); + cv::Mat mono8_img; + if (type.substr(type.length() - 3, 1) == "U"){ + mono8_img = float_img; + rgb_img = cv::Mat(float_img.size(), CV_8UC3); + }else{ + cv::Mat mono8_img = cv::Mat(float_img.size(), CV_8UC1); + if(rgb_img.rows != float_img.rows || rgb_img.cols != float_img.cols){ + rgb_img = cv::Mat(float_img.size(), CV_8UC3); + } + cv::convertScaleAbs(float_img, mono8_img, 255/MAXRANGEIMGD, 0.0); + } + cv::cvtColor(mono8_img, rgb_img, CV_GRAY2RGB); } @@ -79,10 +86,11 @@ namespace Comm { try { //std::cout << image_msg->encoding << std::endl; + //if (image_msg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 || image_msg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0){ - if (image_msg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0){ + if (image_msg->encoding.substr(image_msg->encoding.length() - 2 ) == "C1"){ cv_ptr = cv_bridge::toCvCopy(image_msg); - depthToRGB(cv_ptr->image, img_data); + depthToRGB(cv_ptr->image, img_data, image_msg->encoding); }else{ diff --git a/src/tools/rgbdViewer/CMakeLists.txt b/src/tools/rgbdViewer/CMakeLists.txt index 704cdcb0b..dc011eec0 100644 --- a/src/tools/rgbdViewer/CMakeLists.txt +++ b/src/tools/rgbdViewer/CMakeLists.txt @@ -49,4 +49,5 @@ TARGET_LINK_LIBRARIES(rgbdViewer INSTALL (TARGETS rgbdViewer DESTINATION ${CMAKE_INSTALL_PREFIX}/bin/ COMPONENT rgbdViewer) INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/rgbdViewergui.glade DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/glade COMPONENT rgbdViewer) INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/rgbdViewer.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT rgbdViewer) +INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/rgbdViewerKinect2.yml DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT rgbdViewer) INSTALL (FILES ${CMAKE_CURRENT_SOURCE_DIR}/camera-0.cfg DESTINATION ${CMAKE_INSTALL_PREFIX}/share/jderobot/conf COMPONENT rgbdViewer) diff --git a/src/tools/rgbdViewer/rgbdViewer.cpp b/src/tools/rgbdViewer/rgbdViewer.cpp index 5bcc73a1e..795b464e7 100644 --- a/src/tools/rgbdViewer/rgbdViewer.cpp +++ b/src/tools/rgbdViewer/rgbdViewer.cpp @@ -84,7 +84,7 @@ void *gui_thread(void* arg){ if (camDEPTH){ JdeRobotTypes::Image di = camDEPTH->getImage(); depth = di.data; - std::cout<< di.format << std::endl; + //std::cout<< di.format << std::endl; } } diff --git a/src/tools/rgbdViewer/rgbdViewerKinect2.yml b/src/tools/rgbdViewer/rgbdViewerKinect2.yml new file mode 100644 index 000000000..2deed0251 --- /dev/null +++ b/src/tools/rgbdViewer/rgbdViewerKinect2.yml @@ -0,0 +1,52 @@ +rgbdViewer: + CameraRGB: + Server: ROS # Deactivate, Ice , ROS + Proxy: "cameraA:tcp -h localhost -p 9999" + Format: RGB8 + Topic: "/kinect2/hd/image_color_rect" + Name: cameraA + Fps: 30 + + CameraDEPTH: + Server: ROS # Deactivate, Ice, ROS + Proxy: "cameraB:tcp -h localhost -p 9999" + Format: RGB8 + Topic: "/kinect2/hd/image_depth_rect" + Name: cameraB + Fps: 30 + + PointCloud: + Server: Deactivate # Deactivate, Ice, ROS + Proxy: "pointcloud1:tcp -h localhost -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: pointcloud + Fps: 30 + + RGBD: + Server: Deactivate # Deactivate, Ice, ROS + Proxy: "rgbd1:tcp -h localhost -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: RGBD + Fps: 30 + + Pose3DMotors: + Server: Deactivate # Deactivate, Ice, ROS + Proxy: "Pose3DMotors1:tcp -h 193.147.14.20 -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: Pose3DMotors + + KinectLeds: + Server: Deactivate # Deactivate, Ice, ROS + Proxy: "kinectleds1:tcp -h 193.147.14.20 -p 9999" + Topic: "/TurtlebotROS/cameraL/image_raw" + Name: KinectLeds + + + NodeName: rgbdViewer + #camRGB: "./config/joseMaria/CameraAEsquina.xml" + #camIR: "./config/joseMaria/CameraAEsquina.xml" + #WorldFile: "./config/fempsa/fempsa.cfg" + Width: 640 + Height: 480 + Fps: 15 + Debug: 1 From 6ad17b7276fddeb0242baf0a6588786ade3d014f Mon Sep 17 00:00:00 2001 From: Aitor Martinez Date: Thu, 15 Mar 2018 13:32:54 +0100 Subject: [PATCH 2/2] Updated comm_py to Kinect 2 --- src/libs/comm_py/comm/ros/listenerCamera.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/libs/comm_py/comm/ros/listenerCamera.py b/src/libs/comm_py/comm/ros/listenerCamera.py index c16f09697..29d39c12e 100644 --- a/src/libs/comm_py/comm/ros/listenerCamera.py +++ b/src/libs/comm_py/comm/ros/listenerCamera.py @@ -11,9 +11,9 @@ MINRANGE = 0 -def depthToRGB8(float_img_buff): +def depthToRGB8(float_img_buff, encoding): ''' - Translates from 32FC1 Image format to RGB. Inf values are represented by NaN, when converting to RGB, NaN passed to 0 + Translates from Distance Image format to RGB. Inf values are represented by NaN, when converting to RGB, NaN passed to 0 @param float_img_buff: ROS Image to translate @@ -22,11 +22,15 @@ def depthToRGB8(float_img_buff): @return a Opencv RGB image ''' - float_img = np.zeros((float_img_buff.shape[0], float_img_buff.shape[1], 1), dtype = "float32") - float_img.data = float_img_buff.data + gray_image = None + if (encoding[-3:-2]== "U"): + gray_image = float_img_buff + else: + float_img = np.zeros((float_img_buff.shape[0], float_img_buff.shape[1], 1), dtype = "float32") + float_img.data = float_img_buff.data + gray_image=cv2.convertScaleAbs(float_img, alpha=255/MAXRANGE) - gray_image=cv2.convertScaleAbs(float_img, alpha=255/MAXRANGE) cv_image = cv2.cvtColor(gray_image, cv2.COLOR_GRAY2RGB) return cv_image @@ -52,9 +56,9 @@ def imageMsg2Image(img, bridge): image.format = "RGB8" image.timeStamp = img.header.stamp.secs + (img.header.stamp.nsecs *1e-9) cv_image=0 - if (img.encoding == "32FC1"): - gray_img_buff = bridge.imgmsg_to_cv2(img, "32FC1") - cv_image = depthToRGB8(gray_img_buff) + if (img.encoding[-2:] == "C1"): + gray_img_buff = bridge.imgmsg_to_cv2(img, img.encoding) + cv_image = depthToRGB8(gray_img_buff, img.encoding) else: cv_image = bridge.imgmsg_to_cv2(img, "rgb8") image.data = cv_image