37 #include <boost/assign.hpp> 39 #include <opencv2/opencv.hpp> 46 DiagnosticNodelet::onInit();
48 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
49 dynamic_reconfigure::Server<Config>::CallbackType
f =
52 srv_->setCallback (f);
54 pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
65 names.push_back(
"~input");
66 names.push_back(
"~input/mask");
67 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
76 names.push_back(
"~input");
93 const sensor_msgs::Image::ConstPtr& msg,
94 const sensor_msgs::Image::ConstPtr& mask_msg)
103 const float* histRange = { range };
105 bool uniform =
true;
bool accumulate =
false;
107 cv::calcHist(&image, 1, 0, mask, hist, 1, &
hist_size_,
108 &histRange, uniform, accumulate);
109 jsk_recognition_msgs::ColorHistogram histogram;
110 histogram.header = msg->header;
112 histogram.histogram.push_back(hist.at<
float>(0, i));
118 const sensor_msgs::Image::ConstPtr& msg)
120 compute(msg, sensor_msgs::Image::ConstPtr());
124 Config &config, uint32_t level)
void publish(const boost::shared_ptr< M > &message) const
virtual void configCallback(Config &config, uint32_t level)
SingleChannelHistogramConfig Config
PLUGINLIB_EXPORT_CLASS(jsk_perception::SingleChannelHistogram, nodelet::Nodelet)
virtual void compute(const sensor_msgs::Image::ConstPtr &msg)
std::vector< std::string > V_string
message_filters::Subscriber< sensor_msgs::Image > sub_image_
virtual void unsubscribe()
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
message_filters::Subscriber< sensor_msgs::Image > sub_mask_