feature0d_view.cpp
Go to the documentation of this file.
1 // Copyright (C) 2008-2009 Rosen Diankov
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 // http://www.apache.org/licenses/LICENSE-2.0
7 //
8 // Unless required by applicable law or agreed to in writing, software
9 // distributed under the License is distributed on an "AS IS" BASIS,
10 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
11 // See the License for the specific language governing permissions and
12 // limitations under the License.
15 #include <ros/node_handle.h>
16 #include <ros/master.h>
17 #include <posedetection_msgs/ImageFeature0D.h>
18 
19 #include <opencv2/highgui/highgui.hpp>
20 #include <boost/shared_ptr.hpp>
21 
22 #include <cv_bridge/cv_bridge.h>
23 
24 namespace posedetection_msgs
25 {
27  {
28  std::string topic = _node.resolveName("ImageFeature0D");
29  ros::NodeHandle local_nh("~");
30  local_nh.param("window_name", _window_name, topic);
31  bool autosize;
32  local_nh.param("autosize", autosize, false);
33 
34  _sub = _node.subscribe("ImageFeature0D",1,&Feature0DView::image_cb,this);
35  cvNamedWindow(_window_name.c_str(), autosize ? CV_WINDOW_AUTOSIZE : 0);
36  cvStartWindowThread();
37  }
39 
40  void Feature0DView::image_cb(const posedetection_msgs::ImageFeature0DConstPtr& msg_ptr)
41  {
42  cv_bridge::CvImagePtr cv_ptr;
43  try {
44  cv_ptr = cv_bridge::toCvCopy(msg_ptr->image, "bgr8");
45  cv::Mat image = draw_features(cv_ptr->image,
46  msg_ptr->features.positions,
47  msg_ptr->features.scales,
48  msg_ptr->features.orientations);
49  cv::imshow(_window_name.c_str(), image);
50  }
51  catch (cv_bridge::Exception error) {
52  ROS_WARN("bad frame");
53  return;
54  }
55  }
56 };
57 
58 int main(int argc, char **argv)
59 {
60  ros::init(argc,argv,"feature0d_view");
61  if( !ros::master::check() )
62  return 1;
63 
65  ros::spin();
66  node.reset();
67  return 0;
68 }
ROSCPP_DECL bool check()
void image_cb(const posedetection_msgs::ImageFeature0DConstPtr &msg_ptr)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string resolveName(const std::string &name, bool remap=true) const
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_WARN(...)
ROSCPP_DECL void spin(Spinner &spinner)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
cv::Mat draw_features(const cv::Mat src, const std::vector< float > positions, const std::vector< float > scales, const std::vector< float > orientations)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())


posedetection_msgs
Author(s): Rosen Diankov
autogenerated on Wed Jun 5 2019 20:44:18