41 #include <opencv2/highgui/highgui.hpp> 42 #include <opencv2/imgproc/imgproc.hpp> 44 #include <dynamic_reconfigure/server.h> 45 #include "opencv_apps/RGBColorFilterConfig.h" 46 #include "opencv_apps/HLSColorFilterConfig.h" 47 #include "opencv_apps/HSVColorFilterConfig.h" 62 template <
typename Config>
65 friend class RGBColorFilter;
66 friend class HLSColorFilter;
89 virtual void reconfigureCallback(Config& new_config, uint32_t level) = 0;
91 virtual void filter(
const cv::Mat& input_image, cv::Mat& output_image) = 0;
93 const std::string&
frameWithDefault(
const std::string& frame,
const std::string& image_frame)
102 doWork(msg, cam_info->header.frame_id);
107 doWork(msg, msg->header.frame_id);
110 void doWork(
const sensor_msgs::ImageConstPtr& image_msg,
const std::string& input_frame_from_msg)
120 filter(frame, out_frame);
125 cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
128 std::string new_window_name;
132 if (window_name_ != new_window_name)
134 cv::destroyWindow(window_name_);
135 window_name_ = new_window_name;
137 cv::imshow(window_name_, out_frame);
138 int c = cv::waitKey(1);
142 sensor_msgs::Image::Ptr out_img =
146 catch (cv::Exception& e)
148 NODELET_ERROR(
"Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
154 if (config_.use_camera_info)
155 cam_sub_ = it_->subscribeCamera(
"image", queue_size_, &ColorFilterNodelet::imageCallbackWithInfo,
this);
157 img_sub_ = it_->subscribe(
"image", queue_size_, &ColorFilterNodelet::imageCallback,
this);
173 pnh_->param(
"queue_size", queue_size_, 3);
174 pnh_->param(
"debug_view", debug_view_,
false);
178 always_subscribe_ =
true;
181 window_name_ =
"ColorFilter Demo";
183 reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
184 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
185 boost::bind(&ColorFilterNodelet::reconfigureCallback,
this, _1, _2);
186 reconfigure_server_->setCallback(f);
188 img_pub_ = advertiseImage(*pnh_,
"image", 1);
206 boost::mutex::scoped_lock lock(mutex_);
208 r_max_ = config.r_limit_max;
209 r_min_ = config.r_limit_min;
210 g_max_ = config.g_limit_max;
211 g_min_ = config.g_limit_min;
212 b_max_ = config.b_limit_max;
213 b_min_ = config.b_limit_min;
220 std::swap(r_max_, r_min_);
222 std::swap(g_max_, g_min_);
224 std::swap(b_max_, b_min_);
225 lower_color_range_ = cv::Scalar(b_min_, g_min_, r_min_);
226 upper_color_range_ = cv::Scalar(b_max_, g_max_, r_max_);
229 void filter(
const cv::Mat& input_image, cv::Mat& output_image)
231 cv::inRange(input_image, lower_color_range_, upper_color_range_, output_image);
244 ColorFilterNodelet::onInit();
263 boost::mutex::scoped_lock lock(mutex_);
265 h_max_ = config.h_limit_max;
266 h_min_ = config.h_limit_min;
267 s_max_ = config.s_limit_max;
268 s_min_ = config.s_limit_min;
269 l_max_ = config.l_limit_max;
270 l_min_ = config.l_limit_min;
277 std::swap(s_max_, s_min_);
279 std::swap(l_max_, l_min_);
280 lower_color_range_ = cv::Scalar(h_min_ / 2, l_min_, s_min_, 0);
281 upper_color_range_ = cv::Scalar(h_max_ / 2, l_max_, s_max_, 0);
284 void filter(
const cv::Mat& input_image, cv::Mat& output_image)
287 cv::cvtColor(input_image, hls_image, cv::COLOR_BGR2HLS);
288 if (lower_color_range_[0] < upper_color_range_[0])
290 cv::inRange(hls_image, lower_color_range_, upper_color_range_, output_image);
294 cv::Scalar lower_color_range_0 = cv::Scalar(0, l_min_, s_min_, 0);
295 cv::Scalar upper_color_range_0 = cv::Scalar(h_max_ / 2, l_max_, s_max_, 0);
296 cv::Scalar lower_color_range_360 = cv::Scalar(h_min_ / 2, l_min_, s_min_, 0);
297 cv::Scalar upper_color_range_360 = cv::Scalar(360 / 2, l_max_, s_max_, 0);
298 cv::Mat output_image_0, output_image_360;
299 cv::inRange(hls_image, lower_color_range_0, upper_color_range_0, output_image_0);
300 cv::inRange(hls_image, lower_color_range_360, upper_color_range_360, output_image_360);
301 output_image = output_image_0 | output_image_360;
315 ColorFilterNodelet::onInit();
331 boost::mutex::scoped_lock lock(mutex_);
333 h_max_ = config.h_limit_max;
334 h_min_ = config.h_limit_min;
335 s_max_ = config.s_limit_max;
336 s_min_ = config.s_limit_min;
337 v_max_ = config.v_limit_max;
338 v_min_ = config.v_limit_min;
345 std::swap(s_max_, s_min_);
347 std::swap(v_max_, v_min_);
348 lower_color_range_ = cv::Scalar(h_min_ / 2, s_min_, v_min_, 0);
349 upper_color_range_ = cv::Scalar(h_max_ / 2, s_max_, v_max_, 0);
352 void filter(
const cv::Mat& input_image, cv::Mat& output_image)
355 cv::cvtColor(input_image, hsv_image, cv::COLOR_BGR2HSV);
356 if (lower_color_range_[0] < upper_color_range_[0])
358 cv::inRange(hsv_image, lower_color_range_, upper_color_range_, output_image);
362 cv::Scalar lower_color_range_0 = cv::Scalar(0, s_min_, v_min_, 0);
363 cv::Scalar upper_color_range_0 = cv::Scalar(h_max_ / 2, s_max_, v_max_, 0);
364 cv::Scalar lower_color_range_360 = cv::Scalar(h_min_ / 2, s_min_, v_min_, 0);
365 cv::Scalar upper_color_range_360 = cv::Scalar(360 / 2, s_max_, v_max_, 0);
366 cv::Mat output_image_0, output_image_360;
367 cv::inRange(hsv_image, lower_color_range_0, upper_color_range_0, output_image_0);
368 cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360);
369 output_image = output_image_0 | output_image_360;
383 ColorFilterNodelet::onInit();
396 ROS_WARN(
"DeprecationWarning: Nodelet rgb_color_filter/rgb_color_filter is deprecated, " 397 "and renamed to opencv_apps/rgb_color_filter.");
406 ROS_WARN(
"DeprecationWarning: Nodelet hls_color_filter/hls_color_filter is deprecated, " 407 "and renamed to opencv_apps/hls_color_filter.");
416 ROS_WARN(
"DeprecationWarning: Nodelet hsv_color_filter/hsv_color_filter is deprecated, " 417 "and renamed to opencv_apps/hsv_color_filter.");
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
image_transport::CameraSubscriber cam_sub_
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
#define NODELET_ERROR(...)
void filter(const cv::Mat &input_image, cv::Mat &output_image)
dynamic_reconfigure::Server< Config > ReconfigureServer
boost::shared_ptr< ReconfigureServer > reconfigure_server_
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
Demo code to calculate moments.
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
void filter(const cv::Mat &input_image, cv::Mat &output_image)
void reconfigureCallback(opencv_apps::HLSColorFilterConfig &config, uint32_t level)
cv::Scalar upper_color_range_
virtual void updateCondition()
virtual void updateCondition()
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
void reconfigureCallback(opencv_apps::HSVColorFilterConfig &config, uint32_t level)
void doWork(const sensor_msgs::ImageConstPtr &image_msg, const std::string &input_frame_from_msg)
void publish(const sensor_msgs::Image &message) const
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
boost::shared_ptr< image_transport::ImageTransport > it_
PLUGINLIB_EXPORT_CLASS(color_filter::RGBColorFilterNodelet, nodelet::Nodelet)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
void reconfigureCallback(opencv_apps::RGBColorFilterConfig &config, uint32_t level)
virtual void updateCondition()
image_transport::Publisher img_pub_
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
void filter(const cv::Mat &input_image, cv::Mat &output_image)
cv::Scalar lower_color_range_
image_transport::Subscriber img_sub_
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const