feature0d_view.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2008-2009 Rosen Diankov
00002 // 
00003 // Licensed under the Apache License, Version 2.0 (the "License");
00004 // you may not use this file except in compliance with the License.
00005 // You may obtain a copy of the License at
00006 //     http://www.apache.org/licenses/LICENSE-2.0
00007 // 
00008 // Unless required by applicable law or agreed to in writing, software
00009 // distributed under the License is distributed on an "AS IS" BASIS,
00010 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00011 // See the License for the specific language governing permissions and
00012 // limitations under the License.
00013 #include <ros/node_handle.h>
00014 #include <ros/master.h>
00015 #include <sensor_msgs/Image.h>
00016 #include <sensor_msgs/CameraInfo.h>
00017 #include <posedetection_msgs/ImageFeature0D.h>
00018 
00019 #include <opencv2/highgui/highgui.hpp>
00020 #include <boost/shared_ptr.hpp>
00021 
00022 #include <map>
00023 #include <string>
00024 #include <cstdio>
00025 #include <vector>
00026 
00027 #include <cv_bridge/cv_bridge.h>
00028 
00029 using namespace std;
00030 using namespace ros;
00031 
00032 class Feature0DView
00033 {
00034     ros::NodeHandle _node;
00035     Subscriber _sub;
00036     string _window_name;
00037     cv_bridge::CvImage _bridge;
00038 public:
00039     Feature0DView()
00040     { 
00041         std::string topic = _node.resolveName("ImageFeature0D");
00042         ros::NodeHandle local_nh("~");
00043         local_nh.param("window_name", _window_name, topic);
00044         bool autosize;
00045         local_nh.param("autosize", autosize, false);
00046 
00047         _sub = _node.subscribe("ImageFeature0D",1,&Feature0DView::image_cb,this);
00048         cvNamedWindow(_window_name.c_str(), autosize ? CV_WINDOW_AUTOSIZE : 0);
00049         cvStartWindowThread();
00050     }
00051     virtual ~Feature0DView() {}
00052 
00053     void image_cb(const posedetection_msgs::ImageFeature0DConstPtr& msg_ptr)
00054     {
00055         cv_bridge::CvImagePtr cv_ptr;
00056         try {
00057             cv_ptr = cv_bridge::toCvCopy(msg_ptr->image, "bgr8");
00058             for(size_t i = 0; i < msg_ptr->features.positions.size()/2; ++i) {
00059                 float scale = i < msg_ptr->features.scales.size() ? msg_ptr->features.scales[i] : 10.0;
00060                 CvPoint center = cvPoint(msg_ptr->features.positions[2*i+0],msg_ptr->features.positions[2*i+1]);
00061                 cv::circle(cv_ptr->image,center,scale,CV_RGB(0,255,0));
00062                 if( i < msg_ptr->features.orientations.size() ) {
00063                     // draw line indicating orientation
00064                     cv::line(cv_ptr->image,center,cvPoint(center.x+std::cos(msg_ptr->features.orientations[i])*scale,center.y+std::sin(msg_ptr->features.orientations[i])*scale),CV_RGB(255,0,0));
00065                 }
00066             }
00067             cv::imshow(_window_name.c_str(),cv_ptr->image);
00068         }
00069         catch (cv_bridge::Exception error) {
00070             ROS_WARN("bad frame");
00071             return;
00072         }
00073     }
00074 };
00075 
00076 int main(int argc, char **argv)
00077 {
00078     ros::init(argc,argv,"feature0d_view");
00079     if( !ros::master::check() )
00080         return 1;
00081     
00082     boost::shared_ptr<Feature0DView> node(new Feature0DView());
00083     ros::spin();
00084     node.reset();
00085     return 0;
00086 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


posedetection_msgs
Author(s): Rosen Diankov
autogenerated on Sat Mar 23 2013 12:47:59