CameraROS.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "CameraROS.h"
00029 #include "find_object/Settings.h"
00030 
00031 #include <opencv2/imgproc/imgproc.hpp>
00032 #include <sensor_msgs/image_encodings.h>
00033 
00034 using namespace find_object;
00035 
00036 CameraROS::CameraROS(bool subscribeDepth, QObject * parent) :
00037         Camera(parent),
00038         subscribeDepth_(subscribeDepth)
00039 {
00040         ros::NodeHandle nh; // public
00041         ros::NodeHandle pnh("~"); // private
00042 
00043         qRegisterMetaType<ros::Time>("ros::Time");
00044         qRegisterMetaType<cv::Mat>("cv::Mat");
00045 
00046         if(!subscribeDepth_)
00047         {
00048                 image_transport::ImageTransport it(nh);
00049                 imageSub_ = it.subscribe(nh.resolveName("image"), 1, &CameraROS::imgReceivedCallback, this);
00050         }
00051         else
00052         {
00053                 int queueSize = 10;
00054                 pnh.param("queue_size", queueSize, queueSize);
00055                 ROS_INFO("find_object_ros: queue_size = %d", queueSize);
00056 
00057                 ros::NodeHandle rgb_nh(nh, "rgb");
00058                 ros::NodeHandle rgb_pnh(pnh, "rgb");
00059                 ros::NodeHandle depth_nh(nh, "depth_registered");
00060                 ros::NodeHandle depth_pnh(pnh, "depth_registered");
00061                 image_transport::ImageTransport rgb_it(rgb_nh);
00062                 image_transport::ImageTransport depth_it(depth_nh);
00063                 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00064                 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00065 
00066                 rgbSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect_color"), 1, hintsRgb);
00067                 depthSub_.subscribe(depth_it, depth_nh.resolveName("image_raw"), 1, hintsDepth);
00068                 cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
00069                 sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_);
00070                 sync_->registerCallback(boost::bind(&CameraROS::imgDepthReceivedCallback, this, _1, _2, _3));
00071 
00072         }
00073 }
00074 
00075 QStringList CameraROS::subscribedTopics() const
00076 {
00077         QStringList topics;
00078         if(subscribeDepth_)
00079         {
00080                 topics.append(rgbSub_.getTopic().c_str());
00081                 topics.append(depthSub_.getTopic().c_str());
00082                 topics.append(cameraInfoSub_.getTopic().c_str());
00083         }
00084         else
00085         {
00086                 topics.append(imageSub_.getTopic().c_str());
00087         }
00088         return topics;
00089 }
00090 
00091 bool CameraROS::start()
00092 {
00093         this->startTimer();
00094         return true;
00095 }
00096 
00097 void CameraROS::stop()
00098 {
00099         this->stopTimer();
00100 }
00101 
00102 void CameraROS::takeImage()
00103 {
00104         ros::spinOnce();
00105 }
00106 
00107 void CameraROS::imgReceivedCallback(const sensor_msgs::ImageConstPtr & msg)
00108 {
00109         if(msg->data.size())
00110         {
00111                 cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(msg);
00112                 if(msg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
00113                 {
00114                         cv::Mat cpy = ptr->image.clone();
00115                         Q_EMIT rosDataReceived(msg->header.frame_id, msg->header.stamp, cv::Mat(), 0.0f);
00116                         Q_EMIT imageReceived(cpy);
00117                 }
00118                 else if(msg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)
00119                 {
00120                         cv::Mat bgr;
00121                         cv::cvtColor(ptr->image, bgr, cv::COLOR_RGB2BGR);
00122                         Q_EMIT rosDataReceived(msg->header.frame_id, msg->header.stamp, cv::Mat(), 0.0f);
00123                         Q_EMIT imageReceived(bgr);
00124                 }
00125                 else
00126                 {
00127                         ROS_ERROR("find_object_ros: Encoding \"%s\" detected. Supported image encodings are bgr8 and rgb8...", msg->encoding.c_str());
00128                 }
00129         }
00130 }
00131 
00132 void CameraROS::imgDepthReceivedCallback(
00133                 const sensor_msgs::ImageConstPtr& rgbMsg,
00134                 const sensor_msgs::ImageConstPtr& depthMsg,
00135                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
00136 {
00137         if(!(rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00138                  rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00139                  rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
00140                 (depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
00141                  depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
00142         {
00143                         ROS_ERROR("find_object_ros: Input type must be rgb=mono8,rgb8,bgr8 and depth=32FC1,16UC1");
00144                         return;
00145         }
00146 
00147         if(rgbMsg->data.size())
00148         {
00149                 cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(rgbMsg);
00150                 cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
00151                 float depthConstant = 1.0f/cameraInfoMsg->K[4];
00152                 if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
00153                 {
00154                         cv::Mat cpy = ptr->image.clone();
00155                         Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant);
00156                         Q_EMIT imageReceived(cpy);
00157                 }
00158                 else if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)
00159                 {
00160                         cv::Mat bgr;
00161                         cv::cvtColor(ptr->image, bgr, cv::COLOR_RGB2BGR);
00162                         Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant);
00163                         Q_EMIT imageReceived(bgr);
00164                 }
00165         }
00166 
00167 
00168 }


find_object_2d
Author(s): Mathieu Labbe
autogenerated on Fri Feb 12 2016 01:18:17