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
00029
00030
00031
00032
00033
00034 #include <boost/make_shared.hpp>
00035 #include <boost/version.hpp>
00036 #if ((BOOST_VERSION / 100) % 1000) >= 53
00037 #include <boost/thread/lock_guard.hpp>
00038 #endif
00039
00040 #include <ros/ros.h>
00041 #include <nodelet/nodelet.h>
00042 #include <image_transport/image_transport.h>
00043 #include <sensor_msgs/image_encodings.h>
00044 #include <dynamic_reconfigure/server.h>
00045 #include <image_proc/DebayerConfig.h>
00046
00047 #include <opencv2/imgproc/imgproc.hpp>
00048
00049 #include "edge_aware.h"
00050
00051 #include <cv_bridge/cv_bridge.h>
00052
00053 namespace image_proc {
00054
00055 namespace enc = sensor_msgs::image_encodings;
00056
00057 class DebayerNodelet : public nodelet::Nodelet
00058 {
00059
00060 boost::shared_ptr<image_transport::ImageTransport> it_;
00061 image_transport::Subscriber sub_raw_;
00062
00063 boost::mutex connect_mutex_;
00064 image_transport::Publisher pub_mono_;
00065 image_transport::Publisher pub_color_;
00066
00067
00068 boost::recursive_mutex config_mutex_;
00069 typedef image_proc::DebayerConfig Config;
00070 typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00071 boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00072 Config config_;
00073
00074 virtual void onInit();
00075
00076 void connectCb();
00077
00078 void imageCb(const sensor_msgs::ImageConstPtr& raw_msg);
00079
00080 void configCb(Config &config, uint32_t level);
00081 };
00082
00083 void DebayerNodelet::onInit()
00084 {
00085 ros::NodeHandle &nh = getNodeHandle();
00086 ros::NodeHandle &private_nh = getPrivateNodeHandle();
00087 it_.reset(new image_transport::ImageTransport(nh));
00088
00089
00090 reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
00091 ReconfigureServer::CallbackType f = boost::bind(&DebayerNodelet::configCb, this, _1, _2);
00092 reconfigure_server_->setCallback(f);
00093
00094
00095 typedef image_transport::SubscriberStatusCallback ConnectCB;
00096 ConnectCB connect_cb = boost::bind(&DebayerNodelet::connectCb, this);
00097
00098 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00099 pub_mono_ = it_->advertise("image_mono", 1, connect_cb, connect_cb);
00100 pub_color_ = it_->advertise("image_color", 1, connect_cb, connect_cb);
00101 }
00102
00103
00104 void DebayerNodelet::connectCb()
00105 {
00106 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00107 if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0)
00108 sub_raw_.shutdown();
00109 else if (!sub_raw_)
00110 {
00111 image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00112 sub_raw_ = it_->subscribe("image_raw", 1, &DebayerNodelet::imageCb, this, hints);
00113 }
00114 }
00115
00116 void DebayerNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg)
00117 {
00118 int bit_depth = enc::bitDepth(raw_msg->encoding);
00119
00120 if (raw_msg->encoding == enc::YUV422)
00121 bit_depth = 8;
00122
00123
00124 if (pub_mono_.getNumSubscribers())
00125 {
00126 if (enc::isMono(raw_msg->encoding))
00127 pub_mono_.publish(raw_msg);
00128 else
00129 {
00130 if ((bit_depth != 8) && (bit_depth != 16))
00131 {
00132 NODELET_WARN_THROTTLE(30,
00133 "Raw image data from topic '%s' has unsupported depth: %d",
00134 sub_raw_.getTopic().c_str(), bit_depth);
00135 } else {
00136
00137
00138 sensor_msgs::ImagePtr gray_msg;
00139 try
00140 {
00141 if (bit_depth == 8)
00142 gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO8)->toImageMsg();
00143 else
00144 gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO16)->toImageMsg();
00145 pub_mono_.publish(gray_msg);
00146 }
00147 catch (cv_bridge::Exception &e)
00148 {
00149 NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what());
00150 }
00151 }
00152 }
00153 }
00154
00155
00156 if (!pub_color_.getNumSubscribers())
00157 return;
00158
00159 if (enc::isMono(raw_msg->encoding))
00160 {
00161
00162 pub_color_.publish(raw_msg);
00163
00164
00165 NODELET_WARN_THROTTLE(30,
00166 "Color topic '%s' requested, but raw image data from topic '%s' is grayscale",
00167 pub_color_.getTopic().c_str(), sub_raw_.getTopic().c_str());
00168 }
00169 else if (enc::isColor(raw_msg->encoding))
00170 {
00171 pub_color_.publish(raw_msg);
00172 }
00173 else if (enc::isBayer(raw_msg->encoding)) {
00174 int type = bit_depth == 8 ? CV_8U : CV_16U;
00175 const cv::Mat bayer(raw_msg->height, raw_msg->width, CV_MAKETYPE(type, 1),
00176 const_cast<uint8_t*>(&raw_msg->data[0]), raw_msg->step);
00177
00178 sensor_msgs::ImagePtr color_msg = boost::make_shared<sensor_msgs::Image>();
00179 color_msg->header = raw_msg->header;
00180 color_msg->height = raw_msg->height;
00181 color_msg->width = raw_msg->width;
00182 color_msg->encoding = bit_depth == 8? enc::BGR8 : enc::BGR16;
00183 color_msg->step = color_msg->width * 3 * (bit_depth / 8);
00184 color_msg->data.resize(color_msg->height * color_msg->step);
00185
00186 cv::Mat color(color_msg->height, color_msg->width, CV_MAKETYPE(type, 3),
00187 &color_msg->data[0], color_msg->step);
00188
00189 int algorithm;
00190 {
00191 boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00192 algorithm = config_.debayer;
00193 }
00194
00195 if (algorithm == Debayer_EdgeAware ||
00196 algorithm == Debayer_EdgeAwareWeighted)
00197 {
00198
00199 if (raw_msg->encoding != enc::BAYER_GRBG8)
00200 {
00201 NODELET_WARN_THROTTLE(30, "Edge aware algorithms currently only support GRBG8 Bayer. "
00202 "Falling back to bilinear interpolation.");
00203 algorithm = Debayer_Bilinear;
00204 }
00205 else
00206 {
00207 if (algorithm == Debayer_EdgeAware)
00208 debayerEdgeAware(bayer, color);
00209 else
00210 debayerEdgeAwareWeighted(bayer, color);
00211 }
00212 }
00213 if (algorithm == Debayer_Bilinear ||
00214 algorithm == Debayer_VNG)
00215 {
00216 int code = -1;
00217 if (raw_msg->encoding == enc::BAYER_RGGB8 ||
00218 raw_msg->encoding == enc::BAYER_RGGB16)
00219 code = CV_BayerBG2BGR;
00220 else if (raw_msg->encoding == enc::BAYER_BGGR8 ||
00221 raw_msg->encoding == enc::BAYER_BGGR16)
00222 code = CV_BayerRG2BGR;
00223 else if (raw_msg->encoding == enc::BAYER_GBRG8 ||
00224 raw_msg->encoding == enc::BAYER_GBRG16)
00225 code = CV_BayerGR2BGR;
00226 else if (raw_msg->encoding == enc::BAYER_GRBG8 ||
00227 raw_msg->encoding == enc::BAYER_GRBG16)
00228 code = CV_BayerGB2BGR;
00229
00230 if (algorithm == Debayer_VNG)
00231 code += CV_BayerBG2BGR_VNG - CV_BayerBG2BGR;
00232
00233 cv::cvtColor(bayer, color, code);
00234 }
00235
00236 pub_color_.publish(color_msg);
00237 }
00238 else if (raw_msg->encoding == enc::YUV422)
00239 {
00240
00241 sensor_msgs::ImagePtr color_msg;
00242 try
00243 {
00244 color_msg = cv_bridge::toCvCopy(raw_msg, enc::BGR8)->toImageMsg();
00245 pub_color_.publish(color_msg);
00246 }
00247 catch (cv_bridge::Exception &e)
00248 {
00249 NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what());
00250 }
00251 }
00252 else if (raw_msg->encoding == enc::TYPE_8UC3)
00253 {
00254
00255 NODELET_ERROR_THROTTLE(10,
00256 "Raw image topic '%s' has ambiguous encoding '8UC3'. The "
00257 "source should set the encoding to 'bgr8' or 'rgb8'.",
00258 sub_raw_.getTopic().c_str());
00259 }
00260 else
00261 {
00262 NODELET_ERROR_THROTTLE(10, "Raw image topic '%s' has unsupported encoding '%s'",
00263 sub_raw_.getTopic().c_str(), raw_msg->encoding.c_str());
00264 }
00265 }
00266
00267 void DebayerNodelet::configCb(Config &config, uint32_t level)
00268 {
00269 config_ = config;
00270 }
00271
00272 }
00273
00274
00275 #include <pluginlib/class_list_macros.h>
00276 PLUGINLIB_EXPORT_CLASS( image_proc::DebayerNodelet, nodelet::Nodelet)