feature0d_to_image.cpp
Go to the documentation of this file.
00001 // vim: set tabstop=4 shiftwidth=4:
00002 // Copyright (C) 2008-2009 Rosen Diankov
00003 // 
00004 // Licensed under the Apache License, Version 2.0 (the "License");
00005 // you may not use this file except in compliance with the License.
00006 // You may obtain a copy of the License at
00007 //     http://www.apache.org/licenses/LICENSE-2.0
00008 // 
00009 // Unless required by applicable law or agreed to in writing, software
00010 // distributed under the License is distributed on an "AS IS" BASIS,
00011 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00012 // See the License for the specific language governing permissions and
00013 // limitations under the License.
00014 #include "posedetection_msgs/feature0d_to_image.h"
00015 #include <ros/node_handle.h>
00016 #include <sensor_msgs/Image.h>
00017 #include <posedetection_msgs/ImageFeature0D.h>
00018 
00019 #include <opencv2/highgui/highgui.hpp>
00020 #include <boost/shared_ptr.hpp>
00021 
00022 #include <cv_bridge/cv_bridge.h>
00023 #include <message_filters/synchronizer.h>
00024 
00025 namespace posedetection_msgs
00026 {
00027     Feature0DToImage::Feature0DToImage()
00028     {
00029         ros::NodeHandle local_nh("~");
00030 
00031         _pub = _node.advertise<sensor_msgs::Image>(local_nh.resolveName("output"), 1);
00032         _sub_image.subscribe(_node, "image", 1);
00033         _sub_feature.subscribe(_node, "Feature0D", 1);
00034         _sync = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00035         _sync->connectInput(_sub_image, _sub_feature);
00036         _sync->registerCallback(boost::bind(&Feature0DToImage::imagefeature_cb, this, _1, _2));
00037         _sub_imagefeature = _node.subscribe("ImageFeature0D", 1, &Feature0DToImage::imagefeature_cb, this);
00038     }
00039     Feature0DToImage::~Feature0DToImage() {}
00040 
00041     void Feature0DToImage::imagefeature_cb(const sensor_msgs::ImageConstPtr& image_msg,
00042                                            const posedetection_msgs::Feature0DConstPtr& feature_msg)
00043     {
00044         cv_bridge::CvImagePtr cv_ptr;
00045         try {
00046             cv_ptr = cv_bridge::toCvCopy(image_msg, "bgr8");
00047             cv::Mat image = draw_features(cv_ptr->image,
00048                                           feature_msg->positions,
00049                                           feature_msg->scales,
00050                                           feature_msg->orientations);
00051             _pub.publish(cv_bridge::CvImage(cv_ptr->header, "bgr8", image));
00052         } catch (cv_bridge::Exception& error) {
00053             ROS_WARN("bad frame");
00054             return;
00055         }
00056     }
00057 
00058     void Feature0DToImage::imagefeature_cb(const posedetection_msgs::ImageFeature0DConstPtr& msg_ptr)
00059     {
00060         cv_bridge::CvImagePtr cv_ptr;
00061         try {
00062             cv_ptr = cv_bridge::toCvCopy(msg_ptr->image, "bgr8");
00063             cv::Mat image = draw_features(cv_ptr->image,
00064                                           msg_ptr->features.positions,
00065                                           msg_ptr->features.scales,
00066                                           msg_ptr->features.orientations);
00067             _pub.publish(cv_bridge::CvImage(cv_ptr->header, "bgr8", image));
00068         } catch (cv_bridge::Exception& error) {
00069             ROS_WARN("bad frame");
00070             return;
00071         }
00072     }
00073 }
00074 
00075 int main(int argc, char **argv)
00076 {
00077     ros::init(argc, argv, "feature0d_to_image");
00078     boost::shared_ptr<posedetection_msgs::Feature0DToImage> node(new posedetection_msgs::Feature0DToImage());
00079     ros::spin();
00080     return 0;
00081 }
00082 


posedetection_msgs
Author(s): Rosen Diankov
autogenerated on Sat Jun 8 2019 19:19:23