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