Go to the documentation of this file.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
00036 #include <ros/ros.h>
00037 #include <nodelet/nodelet.h>
00038 #include <geometry_msgs/PolygonStamped.h>
00039 #include <jsk_recognition_msgs/ColorHistogram.h>
00040 #include <sensor_msgs/image_encodings.h>
00041 #include <cv_bridge/cv_bridge.h>
00042 #include <image_transport/image_transport.h>
00043 #include <image_transport/subscriber_filter.h>
00044 #include <message_filters/subscriber.h>
00045 #include <message_filters/synchronizer.h>
00046 #include <message_filters/sync_policies/exact_time.h>
00047 #include <message_filters/sync_policies/approximate_time.h>
00048 #include <dynamic_reconfigure/server.h>
00049 #include <jsk_perception/ColorHistogramConfig.h>
00050 #include "opencv2/highgui/highgui.hpp"
00051 #include "opencv2/imgproc/imgproc.hpp"
00052 #include <jsk_topic_tools/diagnostic_nodelet.h>
00053
00054 namespace jsk_perception
00055 {
00056 class ColorHistogram: public jsk_topic_tools::DiagnosticNodelet
00057 {
00058 public:
00059 typedef message_filters::sync_policies::ApproximateTime<
00060 sensor_msgs::Image,
00061 geometry_msgs::PolygonStamped > SyncPolicy;
00062
00063 typedef message_filters::sync_policies::ApproximateTime<
00064 sensor_msgs::Image,
00065 sensor_msgs::Image> MaskSyncPolicy;
00066 typedef jsk_perception::ColorHistogramConfig Config;
00067 ColorHistogram(): DiagnosticNodelet("ColorHistogram") {}
00068 protected:
00069 boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
00070 boost::shared_ptr<message_filters::Synchronizer<MaskSyncPolicy> > mask_sync_;
00071 boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
00072 image_transport::SubscriberFilter image_sub_;
00073 image_transport::SubscriberFilter image_mask_sub_;
00074 message_filters::Subscriber<geometry_msgs::PolygonStamped> rectangle_sub_;
00075 ros::NodeHandle nh_;
00076 boost::shared_ptr<image_transport::ImageTransport> it_;
00077 ros::Publisher b_hist_pub_, r_hist_pub_, g_hist_pub_,
00078 h_hist_pub_, s_hist_pub_, i_hist_pub_;
00079 ros::Publisher image_pub_;
00080 int b_hist_size_, r_hist_size_, g_hist_size_,
00081 h_hist_size_, s_hist_size_, i_hist_size_;
00082 bool use_mask_;
00083 boost::mutex mutex_;
00084
00085 void configCallback(Config &new_config, uint32_t level);
00086 virtual void onInit();
00087 virtual void subscribe();
00088 virtual void unsubscribe();
00089 virtual void updateDiagnostic(
00090 diagnostic_updater::DiagnosticStatusWrapper &stat);
00091 virtual void convertHistogramToMsg(
00092 const cv::Mat& hist,
00093 int size,
00094 jsk_recognition_msgs::ColorHistogram& msg);
00095 virtual void processBGR(const cv::Mat& bgr_image,
00096 const std_msgs::Header& header);
00097 virtual void processHSI(const cv::Mat& bgr_image,
00098 const std_msgs::Header& header);
00099 virtual void processBGR(const cv::Mat& bgr_image,
00100 const cv::Mat& mask,
00101 const std_msgs::Header& header);
00102 virtual void processHSI(const cv::Mat& bgr_image,
00103 const cv::Mat& mask,
00104 const std_msgs::Header& header);
00105 virtual void extract(
00106 const sensor_msgs::Image::ConstPtr& image,
00107 const geometry_msgs::PolygonStamped::ConstPtr& rectangle);
00108 virtual void extractMask(
00109 const sensor_msgs::Image::ConstPtr& image,
00110 const sensor_msgs::Image::ConstPtr& mask_image);
00111 private:
00112
00113 };
00114 }