edge_detector.cpp
Go to the documentation of this file.
00001 #include <nodelet/nodelet.h>
00002 #include <jsk_topic_tools/log_utils.h>
00003 #include <jsk_perception/EdgeDetectorConfig.h>
00004 #include <image_transport/image_transport.h>
00005 #include <cv_bridge/cv_bridge.h>
00006 #include <sensor_msgs/image_encodings.h>
00007 #include <opencv2/imgproc/imgproc.hpp>
00008 #include "opencv2/highgui/highgui.hpp"
00009 
00010 #include <dynamic_reconfigure/server.h>
00011 
00012 #include <pluginlib/class_list_macros.h>
00013 
00014 namespace enc = sensor_msgs::image_encodings;
00015 
00016 namespace jsk_perception {
00017 class EdgeDetector: public nodelet::Nodelet
00018 {
00019     jsk_perception::EdgeDetectorConfig config_;
00020     dynamic_reconfigure::Server<jsk_perception::EdgeDetectorConfig> srv;
00021 
00022     image_transport::Publisher img_pub_;
00023     image_transport::Subscriber img_sub_;
00024 
00025     boost::shared_ptr<image_transport::ImageTransport> it_;
00026     ros::NodeHandle nh_;
00027     int subscriber_count_;
00028 
00029     double _threshold1;
00030     double _threshold2;
00031     int _apertureSize;
00032     bool _L2gradient;
00033 
00034     void reconfigureCallback(jsk_perception::EdgeDetectorConfig &new_config, uint32_t level)
00035     {
00036         config_ = new_config;
00037         _threshold1 = config_.threshold1;
00038         _threshold2 = config_.threshold2;
00039         _apertureSize = config_.apertureSize;
00040         _L2gradient = config_.L2gradient;
00041         if (subscriber_count_)
00042             { // @todo Could do this without an interruption at some point.
00043                 unsubscribe();
00044                 subscribe();
00045             }
00046     }
00047 
00048     void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00049     {
00050         do_work(msg, msg->header.frame_id);
00051     }
00052 
00053     void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
00054     {
00055         // Transform the image.
00056         try
00057             {
00058                 // Convert the image into something opencv can handle.
00059                 cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image;
00060 
00061                 // Do the work
00062                 cv::Mat out_image;
00063                 if(in_image.channels() >= 3){
00064                   cv::cvtColor(in_image, out_image, CV_BGR2GRAY);
00065                 }else{
00066                   out_image = in_image;
00067                 }
00068                 cv::blur(out_image, out_image, cv::Size(_apertureSize,_apertureSize));
00069                 cv::Canny(out_image, out_image, _threshold1, _threshold2, _apertureSize, _L2gradient);
00070 
00071                 // Publish the image.
00072                 sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, enc::MONO8, out_image).toImageMsg();
00073                 img_pub_.publish(out_img);
00074             }
00075         catch (cv::Exception &e)
00076             {
00077                 JSK_NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00078             }
00079     }
00080 
00081     void subscribe()
00082     {
00083         JSK_NODELET_DEBUG("Subscribing to image topic.");
00084         img_sub_ = it_->subscribe("image", 3, &EdgeDetector::imageCallback, this);
00085     }
00086 
00087     void unsubscribe()
00088     {
00089         JSK_NODELET_DEBUG("Unsubscribing from image topic.");
00090         img_sub_.shutdown();
00091     }
00092 
00093     void connectCb(const image_transport::SingleSubscriberPublisher& ssp)
00094     {
00095         if (subscriber_count_++ == 0) {
00096             subscribe();
00097         }
00098     }
00099 
00100     void disconnectCb(const image_transport::SingleSubscriberPublisher&)
00101     {
00102         subscriber_count_--;
00103         if (subscriber_count_ == 0) {
00104             unsubscribe();
00105         }
00106     }
00107 
00108 public:
00109     void onInit() {
00110       nh_ = getNodeHandle();
00111       subscriber_count_ = 0;
00112       dynamic_reconfigure::Server<jsk_perception::EdgeDetectorConfig>::CallbackType f =
00113         boost::bind(&EdgeDetector::reconfigureCallback, this, _1, _2);
00114       srv.setCallback(f);
00115       it_.reset(new image_transport::ImageTransport(nh_));
00116       image_transport::SubscriberStatusCallback connect_cb    = boost::bind(&EdgeDetector::connectCb, this, _1);
00117       image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&EdgeDetector::disconnectCb, this, _1);
00118       img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "edge")).advertise("image", 1, connect_cb, disconnect_cb);
00119       
00120     }
00121   
00122 };
00123 }
00124 
00125 typedef jsk_perception::EdgeDetector EdgeDetector;
00126 PLUGINLIB_DECLARE_CLASS (jsk_perception, EdgeDetector, EdgeDetector, nodelet::Nodelet);


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Wed Sep 16 2015 04:36:15