CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Always publish /aruco_pose/debug

Browse Source
This commit is contained in:
Oleg Kalachev
2017-12-07 01:19:26 +03:00
parent 1a6e376b1e
commit df1ae9e748
1 changed files with 10 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
aruco_pose/src/aruco_pose.cpp
View File

@@ -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) {