[#277] Added support for kinect 2 with ROS

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{

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

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)

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;
 
 				}
 			}

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