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
00035 #include <ros/ros.h>
00036 #include "opencv_apps/nodelet.h"
00037 #include <image_transport/image_transport.h>
00038 #include <sensor_msgs/image_encodings.h>
00039 #include <cv_bridge/cv_bridge.h>
00040
00041 #include <opencv2/highgui/highgui.hpp>
00042 #include <opencv2/imgproc/imgproc.hpp>
00043
00044 #include <dynamic_reconfigure/server.h>
00045 #include "opencv_apps/RGBColorFilterConfig.h"
00046 #include "opencv_apps/HLSColorFilterConfig.h"
00047 #include "opencv_apps/HSVColorFilterConfig.h"
00048
00049 namespace color_filter
00050 {
00051 class RGBColorFilterNodelet;
00052 class HLSColorFilterNodelet;
00053 class HSVColorFilterNodelet;
00054 }
00055
00056 namespace opencv_apps
00057 {
00058 class RGBColorFilter;
00059 class HLSColorFilter;
00060 class HSVColorFilter;
00061
00062 template <typename Config>
00063 class ColorFilterNodelet : public opencv_apps::Nodelet
00064 {
00065 friend class RGBColorFilter;
00066 friend class HLSColorFilter;
00067
00068 protected:
00069 image_transport::Publisher img_pub_;
00070 image_transport::Subscriber img_sub_;
00071 image_transport::CameraSubscriber cam_sub_;
00072
00073 boost::shared_ptr<image_transport::ImageTransport> it_;
00074
00075 typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00076 Config config_;
00077 boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00078
00079 int queue_size_;
00080 bool debug_view_;
00081
00082 std::string window_name_;
00083
00084 cv::Scalar lower_color_range_;
00085 cv::Scalar upper_color_range_;
00086
00087 boost::mutex mutex_;
00088
00089 virtual void reconfigureCallback(Config& new_config, uint32_t level) = 0;
00090
00091 virtual void filter(const cv::Mat& input_image, cv::Mat& output_image) = 0;
00092
00093 const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame)
00094 {
00095 if (frame.empty())
00096 return image_frame;
00097 return frame;
00098 }
00099
00100 void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
00101 {
00102 doWork(msg, cam_info->header.frame_id);
00103 }
00104
00105 void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00106 {
00107 doWork(msg, msg->header.frame_id);
00108 }
00109
00110 void doWork(const sensor_msgs::ImageConstPtr& image_msg, const std::string& input_frame_from_msg)
00111 {
00112
00113 try
00114 {
00115
00116 cv::Mat frame = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8)->image;
00117
00118
00119 cv::Mat out_frame;
00120 filter(frame, out_frame);
00121
00123 if (debug_view_)
00124 {
00125 cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
00126 }
00127
00128 std::string new_window_name;
00129
00130 if (debug_view_)
00131 {
00132 if (window_name_ != new_window_name)
00133 {
00134 cv::destroyWindow(window_name_);
00135 window_name_ = new_window_name;
00136 }
00137 cv::imshow(window_name_, out_frame);
00138 int c = cv::waitKey(1);
00139 }
00140
00141
00142 sensor_msgs::Image::Ptr out_img =
00143 cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::MONO8, out_frame).toImageMsg();
00144 img_pub_.publish(out_img);
00145 }
00146 catch (cv::Exception& e)
00147 {
00148 NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00149 }
00150 }
00151 void subscribe()
00152 {
00153 NODELET_DEBUG("Subscribing to image topic.");
00154 if (config_.use_camera_info)
00155 cam_sub_ = it_->subscribeCamera("image", queue_size_, &ColorFilterNodelet::imageCallbackWithInfo, this);
00156 else
00157 img_sub_ = it_->subscribe("image", queue_size_, &ColorFilterNodelet::imageCallback, this);
00158 }
00159
00160 void unsubscribe()
00161 {
00162 NODELET_DEBUG("Unsubscribing from image topic.");
00163 img_sub_.shutdown();
00164 cam_sub_.shutdown();
00165 }
00166
00167 public:
00168 virtual void onInit()
00169 {
00170 Nodelet::onInit();
00171 it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));
00172
00173 pnh_->param("queue_size", queue_size_, 3);
00174 pnh_->param("debug_view", debug_view_, false);
00175
00176 if (debug_view_)
00177 {
00178 always_subscribe_ = true;
00179 }
00180
00181 window_name_ = "ColorFilter Demo";
00182
00183 reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
00184 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00185 boost::bind(&ColorFilterNodelet::reconfigureCallback, this, _1, _2);
00186 reconfigure_server_->setCallback(f);
00187
00188 img_pub_ = advertiseImage(*pnh_, "image", 1);
00189
00190 onInitPostProcess();
00191 }
00192 };
00193
00194 class RGBColorFilterNodelet : public ColorFilterNodelet<opencv_apps::RGBColorFilterConfig>
00195 {
00196 protected:
00197 int r_min_;
00198 int r_max_;
00199 int b_min_;
00200 int b_max_;
00201 int g_min_;
00202 int g_max_;
00203
00204 void reconfigureCallback(opencv_apps::RGBColorFilterConfig& config, uint32_t level)
00205 {
00206 boost::mutex::scoped_lock lock(mutex_);
00207 config_ = config;
00208 r_max_ = config.r_limit_max;
00209 r_min_ = config.r_limit_min;
00210 g_max_ = config.g_limit_max;
00211 g_min_ = config.g_limit_min;
00212 b_max_ = config.b_limit_max;
00213 b_min_ = config.b_limit_min;
00214 updateCondition();
00215 }
00216
00217 virtual void updateCondition()
00218 {
00219 if (r_max_ < r_min_)
00220 std::swap(r_max_, r_min_);
00221 if (g_max_ < g_min_)
00222 std::swap(g_max_, g_min_);
00223 if (b_max_ < b_min_)
00224 std::swap(b_max_, b_min_);
00225 lower_color_range_ = cv::Scalar(b_min_, g_min_, r_min_);
00226 upper_color_range_ = cv::Scalar(b_max_, g_max_, r_max_);
00227 }
00228
00229 void filter(const cv::Mat& input_image, cv::Mat& output_image)
00230 {
00231 cv::inRange(input_image, lower_color_range_, upper_color_range_, output_image);
00232 }
00233
00234 protected:
00235 virtual void onInit()
00236 {
00237 r_max_ = 255;
00238 r_min_ = 0;
00239 g_max_ = 255;
00240 g_min_ = 0;
00241 b_max_ = 255;
00242 b_min_ = 0;
00243
00244 ColorFilterNodelet::onInit();
00245 }
00246 friend class color_filter::RGBColorFilterNodelet;
00247 friend class color_filter::HLSColorFilterNodelet;
00248 friend class color_filter::HSVColorFilterNodelet;
00249 };
00250
00251 class HLSColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HLSColorFilterConfig>
00252 {
00253 protected:
00254 int h_min_;
00255 int h_max_;
00256 int s_min_;
00257 int s_max_;
00258 int l_min_;
00259 int l_max_;
00260
00261 void reconfigureCallback(opencv_apps::HLSColorFilterConfig& config, uint32_t level)
00262 {
00263 boost::mutex::scoped_lock lock(mutex_);
00264 config_ = config;
00265 h_max_ = config.h_limit_max;
00266 h_min_ = config.h_limit_min;
00267 s_max_ = config.s_limit_max;
00268 s_min_ = config.s_limit_min;
00269 l_max_ = config.l_limit_max;
00270 l_min_ = config.l_limit_min;
00271 updateCondition();
00272 }
00273
00274 virtual void updateCondition()
00275 {
00276 if (s_max_ < s_min_)
00277 std::swap(s_max_, s_min_);
00278 if (l_max_ < l_min_)
00279 std::swap(l_max_, l_min_);
00280 lower_color_range_ = cv::Scalar(h_min_ / 2, l_min_, s_min_, 0);
00281 upper_color_range_ = cv::Scalar(h_max_ / 2, l_max_, s_max_, 0);
00282 }
00283
00284 void filter(const cv::Mat& input_image, cv::Mat& output_image)
00285 {
00286 cv::Mat hls_image;
00287 cv::cvtColor(input_image, hls_image, cv::COLOR_BGR2HLS);
00288 if (lower_color_range_[0] < upper_color_range_[0])
00289 {
00290 cv::inRange(hls_image, lower_color_range_, upper_color_range_, output_image);
00291 }
00292 else
00293 {
00294 cv::Scalar lower_color_range_0 = cv::Scalar(0, l_min_, s_min_, 0);
00295 cv::Scalar upper_color_range_0 = cv::Scalar(h_max_ / 2, l_max_, s_max_, 0);
00296 cv::Scalar lower_color_range_360 = cv::Scalar(h_min_ / 2, l_min_, s_min_, 0);
00297 cv::Scalar upper_color_range_360 = cv::Scalar(360 / 2, l_max_, s_max_, 0);
00298 cv::Mat output_image_0, output_image_360;
00299 cv::inRange(hls_image, lower_color_range_0, upper_color_range_0, output_image_0);
00300 cv::inRange(hls_image, lower_color_range_360, upper_color_range_360, output_image_360);
00301 output_image = output_image_0 | output_image_360;
00302 }
00303 }
00304
00305 public:
00306 virtual void onInit()
00307 {
00308 h_max_ = 360;
00309 h_min_ = 0;
00310 s_max_ = 256;
00311 s_min_ = 0;
00312 l_max_ = 256;
00313 l_min_ = 0;
00314
00315 ColorFilterNodelet::onInit();
00316 }
00317 };
00318
00319 class HSVColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HSVColorFilterConfig>
00320 {
00321 protected:
00322 int h_min_;
00323 int h_max_;
00324 int s_min_;
00325 int s_max_;
00326 int v_min_;
00327 int v_max_;
00328
00329 void reconfigureCallback(opencv_apps::HSVColorFilterConfig& config, uint32_t level)
00330 {
00331 boost::mutex::scoped_lock lock(mutex_);
00332 config_ = config;
00333 h_max_ = config.h_limit_max;
00334 h_min_ = config.h_limit_min;
00335 s_max_ = config.s_limit_max;
00336 s_min_ = config.s_limit_min;
00337 v_max_ = config.v_limit_max;
00338 v_min_ = config.v_limit_min;
00339 updateCondition();
00340 }
00341
00342 virtual void updateCondition()
00343 {
00344 if (s_max_ < s_min_)
00345 std::swap(s_max_, s_min_);
00346 if (v_max_ < v_min_)
00347 std::swap(v_max_, v_min_);
00348 lower_color_range_ = cv::Scalar(h_min_ / 2, s_min_, v_min_, 0);
00349 upper_color_range_ = cv::Scalar(h_max_ / 2, s_max_, v_max_, 0);
00350 }
00351
00352 void filter(const cv::Mat& input_image, cv::Mat& output_image)
00353 {
00354 cv::Mat hsv_image;
00355 cv::cvtColor(input_image, hsv_image, cv::COLOR_BGR2HSV);
00356 if (lower_color_range_[0] < upper_color_range_[0])
00357 {
00358 cv::inRange(hsv_image, lower_color_range_, upper_color_range_, output_image);
00359 }
00360 else
00361 {
00362 cv::Scalar lower_color_range_0 = cv::Scalar(0, s_min_, v_min_, 0);
00363 cv::Scalar upper_color_range_0 = cv::Scalar(h_max_ / 2, s_max_, v_max_, 0);
00364 cv::Scalar lower_color_range_360 = cv::Scalar(h_min_ / 2, s_min_, v_min_, 0);
00365 cv::Scalar upper_color_range_360 = cv::Scalar(360 / 2, s_max_, v_max_, 0);
00366 cv::Mat output_image_0, output_image_360;
00367 cv::inRange(hsv_image, lower_color_range_0, upper_color_range_0, output_image_0);
00368 cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360);
00369 output_image = output_image_0 | output_image_360;
00370 }
00371 }
00372
00373 public:
00374 virtual void onInit()
00375 {
00376 h_max_ = 360;
00377 h_min_ = 0;
00378 s_max_ = 256;
00379 s_min_ = 0;
00380 v_max_ = 256;
00381 v_min_ = 0;
00382
00383 ColorFilterNodelet::onInit();
00384 }
00385 };
00386
00387 }
00388
00389 namespace color_filter
00390 {
00391 class RGBColorFilterNodelet : public opencv_apps::RGBColorFilterNodelet
00392 {
00393 public:
00394 virtual void onInit()
00395 {
00396 ROS_WARN("DeprecationWarning: Nodelet rgb_color_filter/rgb_color_filter is deprecated, "
00397 "and renamed to opencv_apps/rgb_color_filter.");
00398 opencv_apps::RGBColorFilterNodelet::onInit();
00399 }
00400 };
00401 class HLSColorFilterNodelet : public opencv_apps::HLSColorFilterNodelet
00402 {
00403 public:
00404 virtual void onInit()
00405 {
00406 ROS_WARN("DeprecationWarning: Nodelet hls_color_filter/hls_color_filter is deprecated, "
00407 "and renamed to opencv_apps/hls_color_filter.");
00408 opencv_apps::HLSColorFilterNodelet::onInit();
00409 }
00410 };
00411 class HSVColorFilterNodelet : public opencv_apps::HSVColorFilterNodelet
00412 {
00413 public:
00414 virtual void onInit()
00415 {
00416 ROS_WARN("DeprecationWarning: Nodelet hsv_color_filter/hsv_color_filter is deprecated, "
00417 "and renamed to opencv_apps/hsv_color_filter.");
00418 opencv_apps::HSVColorFilterNodelet::onInit();
00419 }
00420 };
00421 }
00422
00423 #include <pluginlib/class_list_macros.h>
00424 PLUGINLIB_EXPORT_CLASS(color_filter::RGBColorFilterNodelet, nodelet::Nodelet);
00425 PLUGINLIB_EXPORT_CLASS(color_filter::HLSColorFilterNodelet, nodelet::Nodelet);
00426 PLUGINLIB_EXPORT_CLASS(color_filter::HSVColorFilterNodelet, nodelet::Nodelet);
00427 PLUGINLIB_EXPORT_CLASS(opencv_apps::RGBColorFilterNodelet, nodelet::Nodelet);
00428 PLUGINLIB_EXPORT_CLASS(opencv_apps::HLSColorFilterNodelet, nodelet::Nodelet);
00429 PLUGINLIB_EXPORT_CLASS(opencv_apps::HSVColorFilterNodelet, nodelet::Nodelet);