[adding_images] convert input image to bgr8 encoding and publish bgr8…

… image
ros-perception · Sep 1, 2016 · a1dec87 · a1dec87
1 parent 5b1e20a
commit a1dec87
Showing 1 changed file with 12 additions and 1 deletion.
diff --git a/src/nodelet/adding_images_nodelet.cpp b/src/nodelet/adding_images_nodelet.cpp
@@ -170,6 +170,16 @@ namespace adding_images {
         cv::Mat image2 =
           cv_bridge::toCvShare(image_msg2, image_msg2->encoding)->image;
 
+        // convert image to bgr8
+        if (image_msg1->encoding == sensor_msgs::image_encodings::RGB8 ||
+            image_msg1->encoding == sensor_msgs::image_encodings::RGB16) {
+          cv::cvtColor(image1, image1, CV_RGB2BGR);
+        }
+        if (image_msg2->encoding == sensor_msgs::image_encodings::RGB8 ||
+            image_msg2->encoding == sensor_msgs::image_encodings::RGB16) {
+          cv::cvtColor(image2, image2, CV_RGB2BGR);
+        }
+
         cv::Mat result_image;
         cv::addWeighted(image1, alpha_, image2, beta_, gamma_, result_image);
         //-- Show what you got
@@ -178,8 +188,9 @@ namespace adding_images {
           cv::imshow(window_name_, result_image);
           int c = cv::waitKey(1);
         }
+        // publish bgr8 image
         img_pub_.publish(cv_bridge::CvImage(image_msg1->header,
-                                            image_msg1->encoding,
+                                            sensor_msgs::image_encodings::BGR8,
                                             result_image).toImageMsg());
 
       } catch (cv::Exception& e) {