From ef0c565b7e3b3a3e78ae5424a7536cf3dd3e9101 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Mon, 28 Jan 2019 16:37:35 -0500
Subject: [PATCH] added 3d example with zed, added bgra support on rgb/depth
 callback

---
 launch/find_object_3d_kinect2.launch | 13 -----------
 launch/find_object_3d_zed.launch     | 16 +++++++++++++
 src/ros/CameraROS.cpp                | 34 ++++++++++++++++------------
 3 files changed, 36 insertions(+), 27 deletions(-)
 create mode 100644 launch/find_object_3d_zed.launch

diff --git a/launch/find_object_3d_kinect2.launch b/launch/find_object_3d_kinect2.launch
index 1e4198f0..2cfac046 100644
--- a/launch/find_object_3d_kinect2.launch
+++ b/launch/find_object_3d_kinect2.launch
@@ -16,17 +16,4 @@
 		<remap from="depth_registered/image_raw" to="/kinect2/$(arg resolution)/image_depth_rect"/>
 		<remap from="depth_registered/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
 	</node>
-	
-	<!-- Example of tf synchronisation with the objectsStamped message -->
-	<node name="tf_example" pkg="find_object_2d" type="tf_example" output="screen">
-		<param name="map_frame_id" value="/map" type="string"/>
-		<param name="object_prefix" value="object" type="str"/>
-	</node>
-	<!-- fake some tf frames for the example /map -> /odom -> /base_link -> /kinect2_base_link -->
-	<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" 
-		args="0.1 0.0 0.3 0.0 0.0 0.0 /base_link /kinect2_link 100" />
-	<node pkg="tf" type="static_transform_publisher" name="odom_to_base_tf" 
-		args="1.0 0.0 0.1 1.5707 0.0 0.0 /odom /base_link 100" />
-	<node pkg="tf" type="static_transform_publisher" name="map_to_odom_tf" 
-		args="0.0 0.5 0.0 0.7853 0.0 0.0 /map /odom 100" />
 </launch>
diff --git a/launch/find_object_3d_zed.launch b/launch/find_object_3d_zed.launch
new file mode 100644
index 00000000..3911acf7
--- /dev/null
+++ b/launch/find_object_3d_zed.launch
@@ -0,0 +1,16 @@
+<launch>
+	<!-- Example finding 3D poses of the objects detected -->
+	<!-- $ roslaunch zed_wrapper zed.launch -->
+
+	<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
+		<param name="gui" value="true" type="bool"/>
+		<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
+		<param name="subscribe_depth" value="true" type="bool"/>
+		<param name="objects_path" value="" type="str"/>
+		<param name="object_prefix" value="object" type="str"/>
+		
+		<remap from="rgb/image_rect_color" to="/zed/rgb/image_rect_color"/>
+		<remap from="depth_registered/image_raw" to="/zed/depth/depth_registered"/>
+		<remap from="depth_registered/camera_info" to="/zed/rgb/camera_info"/>
+	</node>
+</launch>
diff --git a/src/ros/CameraROS.cpp b/src/ros/CameraROS.cpp
index 65a15269..2f9eb5ad 100644
--- a/src/ros/CameraROS.cpp
+++ b/src/ros/CameraROS.cpp
@@ -137,13 +137,10 @@ void CameraROS::imgDepthReceivedCallback(
 		const sensor_msgs::ImageConstPtr& depthMsg,
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
 {
-	if(!(rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-		 rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-		 rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
-		(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
-		 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
+	if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
+	   depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0)
 	{
-			ROS_ERROR("find_object_ros: Input type must be rgb=mono8,rgb8,bgr8 and depth=32FC1,16UC1");
+			ROS_ERROR("find_object_ros: Depth image type must be 32FC1 or 16UC1");
 			return;
 	}
 
@@ -152,18 +149,27 @@ void CameraROS::imgDepthReceivedCallback(
 		cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(rgbMsg);
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 		float depthConstant = 1.0f/cameraInfoMsg->K[4];
-		if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
+
+		cv::Mat image;
+		cv_bridge::CvImageConstPtr imgPtr = cv_bridge::toCvShare(rgbMsg);
+		try
 		{
-			cv::Mat cpy = ptr->image.clone();
+			if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+			   rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				image = cv_bridge::cvtColor(imgPtr, "mono8")->image;
+			}
+			else
+			{
+				image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
+			}
+
 			Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant);
-			Q_EMIT imageReceived(cpy);
+			Q_EMIT imageReceived(image);
 		}
-		else if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)
+		catch(const cv_bridge::Exception & e)
 		{
-			cv::Mat bgr;
-			cv::cvtColor(ptr->image, bgr, cv::COLOR_RGB2BGR);
-			Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant);
-			Q_EMIT imageReceived(bgr);
+			ROS_ERROR("find_object_ros: Could not convert input image to mono8 or bgr8 format, encoding detected is %s... cv_bridge exception: %s", rgbMsg->encoding.c_str(), e.what());
 		}
 	}