CameraROS.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "CameraROS.h"
29 #include "find_object/Settings.h"
30 
31 #include <opencv2/imgproc/imgproc.hpp>
33 
34 using namespace find_object;
35 
36 CameraROS::CameraROS(bool subscribeDepth, QObject * parent) :
37  Camera(parent),
38  subscribeDepth_(subscribeDepth)
39 {
40  ros::NodeHandle nh; // public
41  ros::NodeHandle pnh("~"); // private
42 
43  qRegisterMetaType<ros::Time>("ros::Time");
44  qRegisterMetaType<cv::Mat>("cv::Mat");
45 
46  if(!subscribeDepth_)
47  {
50  }
51  else
52  {
53  int queueSize = 10;
54  pnh.param("queue_size", queueSize, queueSize);
55  ROS_INFO("find_object_ros: queue_size = %d", queueSize);
56 
57  ros::NodeHandle rgb_nh(nh, "rgb");
58  ros::NodeHandle rgb_pnh(pnh, "rgb");
59  ros::NodeHandle depth_nh(nh, "depth_registered");
60  ros::NodeHandle depth_pnh(pnh, "depth_registered");
61  image_transport::ImageTransport rgb_it(rgb_nh);
62  image_transport::ImageTransport depth_it(depth_nh);
63  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
64  image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
65 
66  rgbSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect_color"), 1, hintsRgb);
67  depthSub_.subscribe(depth_it, depth_nh.resolveName("image_raw"), 1, hintsDepth);
68  cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
70  sync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3));
71 
72  }
73 }
74 
75 QStringList CameraROS::subscribedTopics() const
76 {
77  QStringList topics;
78  if(subscribeDepth_)
79  {
80  topics.append(rgbSub_.getTopic().c_str());
81  topics.append(depthSub_.getTopic().c_str());
82  topics.append(cameraInfoSub_.getTopic().c_str());
83  }
84  else
85  {
86  topics.append(imageSub_.getTopic().c_str());
87  }
88  return topics;
89 }
90 
92 {
93  this->startTimer();
94  return true;
95 }
96 
98 {
99  this->stopTimer();
100 }
101 
103 {
104  ros::spinOnce();
105 }
106 
107 void CameraROS::imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
108 {
109  if(msg->data.size())
110  {
112  if(msg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
113  {
114  cv::Mat cpy = ptr->image.clone();
115  Q_EMIT rosDataReceived(msg->header.frame_id, msg->header.stamp, cv::Mat(), 0.0f);
116  Q_EMIT imageReceived(cpy);
117  }
118  else if(msg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)
119  {
120  cv::Mat bgr;
121  cv::cvtColor(ptr->image, bgr, cv::COLOR_RGB2BGR);
122  Q_EMIT rosDataReceived(msg->header.frame_id, msg->header.stamp, cv::Mat(), 0.0f);
123  Q_EMIT imageReceived(bgr);
124  }
125  else
126  {
127  ROS_ERROR("find_object_ros: Encoding \"%s\" detected. Supported image encodings are bgr8 and rgb8...", msg->encoding.c_str());
128  }
129  }
130 }
131 
133  const sensor_msgs::ImageConstPtr& rgbMsg,
134  const sensor_msgs::ImageConstPtr& depthMsg,
135  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
136 {
137  if(!(rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
138  rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
139  rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
140  (depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
141  depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
142  {
143  ROS_ERROR("find_object_ros: Input type must be rgb=mono8,rgb8,bgr8 and depth=32FC1,16UC1");
144  return;
145  }
146 
147  if(rgbMsg->data.size())
148  {
151  float depthConstant = 1.0f/cameraInfoMsg->K[4];
152  if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
153  {
154  cv::Mat cpy = ptr->image.clone();
155  Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant);
156  Q_EMIT imageReceived(cpy);
157  }
158  else if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)
159  {
160  cv::Mat bgr;
161  cv::cvtColor(ptr->image, bgr, cv::COLOR_RGB2BGR);
162  Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant);
163  Q_EMIT imageReceived(bgr);
164  }
165  }
166 
167 
168 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
std::string getTopic() const
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > MySyncPolicy
Definition: CameraROS.h:86
void rosDataReceived(const std::string &frameId, const ros::Time &stamp, const cv::Mat &depth, float depthConstant)
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoSub_
Definition: CameraROS.h:81
virtual void takeImage()
Definition: CameraROS.cpp:102
std::string resolveName(const std::string &name, bool remap=true) const
image_transport::SubscriberFilter rgbSub_
Definition: CameraROS.h:79
void imgDepthReceivedCallback(const sensor_msgs::ImageConstPtr &rgbMsg, const sensor_msgs::ImageConstPtr &depthMsg, const sensor_msgs::CameraInfoConstPtr &cameraInfoMsg)
Definition: CameraROS.cpp:132
virtual bool start()
Definition: CameraROS.cpp:91
void imgReceivedCallback(const sensor_msgs::ImageConstPtr &msg)
Definition: CameraROS.cpp:107
void imageReceived(const cv::Mat &image)
image_transport::SubscriberFilter depthSub_
Definition: CameraROS.h:80
std::string getTopic() const
message_filters::Synchronizer< MySyncPolicy > * sync_
Definition: CameraROS.h:87
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
const std::string TYPE_32FC1
CameraROS(bool subscribeDepth, QObject *parent=0)
Definition: CameraROS.cpp:36
const std::string TYPE_16UC1
image_transport::Subscriber imageSub_
Definition: CameraROS.h:77
bool subscribeDepth_
Definition: CameraROS.h:76
QStringList subscribedTopics() const
Definition: CameraROS.cpp:75
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
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)
virtual void stop()
Definition: CameraROS.cpp:97
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
std::string getTopic() const


find_object_2d
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 19:22:26