Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <boost/foreach.hpp>
00003 #include <april_msgs/TagPoseArray.h>
00004
00005 #include <ros/ros.h>
00006 #include <std_msgs/String.h>
00007 #include <image_transport/image_transport.h>
00008 #include <sensor_msgs/Image.h>
00009
00010 #include <opencv/cv.h>
00011 #include <opencv/highgui.h>
00012 #include <cv_bridge/cv_bridge.h>
00013
00014 namespace {
00015 cv_bridge::CvImageConstPtr imageMsgPtr;
00016 bool first_image_received = false;
00017 }
00018
00019 void drawDetections(cv_bridge::CvImageConstPtr& input,
00020 const april_msgs::TagPoseArray::ConstPtr& detections,
00021 cv::Mat& output) {
00022 output = input->image.clone();
00023 BOOST_FOREACH(const april_msgs::TagPose& tag, detections->tags) {
00024 int length = 4;
00025 for (int i = 0; i < length; i++) {
00026 cv::Point p1(tag.image_coordinates[i].x, tag.image_coordinates[i].y);
00027 cv::Point p2(tag.image_coordinates[(i + 1) % length].x,
00028 tag.image_coordinates[(i + 1) % length].y);
00029 cv::line(output, p1, p2, cv::Scalar(255,0,0));
00030 }
00031 }
00032 }
00033
00034
00035 void processDetections(const april_msgs::TagPoseArray::ConstPtr& detections) {
00036 if (first_image_received) {
00037 cv::Mat output;
00038 drawDetections(imageMsgPtr, detections, output);
00039 cv::imshow("Output", output);
00040 }
00041 }
00042
00043 void processImage(const sensor_msgs::ImageConstPtr &msg) {
00044 imageMsgPtr = cv_bridge::toCvShare(msg, "bgr8");
00045 first_image_received = true;
00046 }
00047
00048 int main(int argc, char **argv) {
00049 ros::init(argc, argv, "april_test");
00050
00051 cv::namedWindow("Output");
00052 cvResizeWindow("Output", 320, 240);
00053 cvStartWindowThread();
00054
00055 ros::NodeHandle n;
00056 ros::Subscriber sub = n.subscribe("tags", 1, processDetections);
00057
00058 image_transport::ImageTransport it(n);
00059 std::string image_topic = n.resolveName("usb_cam/image_raw");
00060 image_transport::Subscriber center_camera = it.subscribe(image_topic, 1, &processImage);
00061
00062 ros::spin();
00063 return 0;
00064 }