feature0d_to_image.cpp
Go to the documentation of this file.
1 // vim: set tabstop=4 shiftwidth=4:
2 // Copyright (C) 2008-2009 Rosen Diankov
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
15 #include <ros/node_handle.h>
16 #include <sensor_msgs/Image.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>
24 
25 namespace posedetection_msgs
26 {
28  {
29  ros::NodeHandle local_nh("~");
30 
31  _pub = _node.advertise<sensor_msgs::Image>(local_nh.resolveName("output"), 1);
32  _sub_image.subscribe(_node, "image", 1);
33  _sub_feature.subscribe(_node, "Feature0D", 1);
34  _sync = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
35  _sync->connectInput(_sub_image, _sub_feature);
36  _sync->registerCallback(boost::bind(&Feature0DToImage::imagefeature_cb, this, _1, _2));
38  }
40 
41  void Feature0DToImage::imagefeature_cb(const sensor_msgs::ImageConstPtr& image_msg,
42  const posedetection_msgs::Feature0DConstPtr& feature_msg)
43  {
44  cv_bridge::CvImagePtr cv_ptr;
45  try {
46  cv_ptr = cv_bridge::toCvCopy(image_msg, "bgr8");
47  cv::Mat image = draw_features(cv_ptr->image,
48  feature_msg->positions,
49  feature_msg->scales,
50  feature_msg->orientations);
51  _pub.publish(cv_bridge::CvImage(cv_ptr->header, "bgr8", image));
52  } catch (cv_bridge::Exception& error) {
53  ROS_WARN("bad frame");
54  return;
55  }
56  }
57 
58  void Feature0DToImage::imagefeature_cb(const posedetection_msgs::ImageFeature0DConstPtr& msg_ptr)
59  {
60  cv_bridge::CvImagePtr cv_ptr;
61  try {
62  cv_ptr = cv_bridge::toCvCopy(msg_ptr->image, "bgr8");
63  cv::Mat image = draw_features(cv_ptr->image,
64  msg_ptr->features.positions,
65  msg_ptr->features.scales,
66  msg_ptr->features.orientations);
67  _pub.publish(cv_bridge::CvImage(cv_ptr->header, "bgr8", image));
68  } catch (cv_bridge::Exception& error) {
69  ROS_WARN("bad frame");
70  return;
71  }
72  }
73 }
74 
75 int main(int argc, char **argv)
76 {
77  ros::init(argc, argv, "feature0d_to_image");
79  ros::spin();
80  return 0;
81 }
82 
message_filters::Subscriber< posedetection_msgs::Feature0D > _sub_feature
void publish(const boost::shared_ptr< M > &message) const
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
void imagefeature_cb(const posedetection_msgs::ImageFeature0DConstPtr &msg_ptr)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > _sync
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
#define ROS_WARN(...)
ROSCPP_DECL void spin(Spinner &spinner)
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())
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
message_filters::Subscriber< sensor_msgs::Image > _sub_image


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