From df1ae9e74863ee8ad4ae699d6bdd3966355716e8 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 7 Dec 2017 01:19:26 +0300 Subject: [PATCH] Always publish /aruco_pose/debug --- aruco_pose/src/aruco_pose.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/aruco_pose/src/aruco_pose.cpp b/aruco_pose/src/aruco_pose.cpp index 77f17d80..54eeaf5d 100644 --- a/aruco_pose/src/aruco_pose.cpp +++ b/aruco_pose/src/aruco_pose.cpp @@ -212,15 +212,19 @@ void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs: // Publish debug image cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3); - cv_bridge::CvImage out_msg; - out_msg.header.frame_id = msg->header.frame_id; - out_msg.header.stamp = msg->header.stamp; - out_msg.encoding = sensor_msgs::image_encodings::BGR8; - out_msg.image = image; - img_pub.publish(out_msg.toImageMsg()); } } } + + if (img_pub.getNumSubscribers() > 0) + { + cv_bridge::CvImage out_msg; + out_msg.header.frame_id = msg->header.frame_id; + out_msg.header.stamp = msg->header.stamp; + out_msg.encoding = sensor_msgs::image_encodings::BGR8; + out_msg.image = image; + img_pub.publish(out_msg.toImageMsg()); + } } void ArucoPose::parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo, cv::Mat &cameraMat, cv::Mat &distCoeffs) {