00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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;
00041 ros::NodeHandle pnh("~");
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 }